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;
     }
@@ -465,6 +465,12 @@ public:
 		//for kinematic objects, we could also use use:
 		//		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)
 	{

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,31 +898,53 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 		// calculate zhat_i^A
 		//
-		//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;
+		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;
 
-		zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
+			zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
 
 #if 0	
-		{
+			{
 
-			b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
-			i+1,
-			zeroAccSpatFrc[i+1].m_topVec[0],
-			zeroAccSpatFrc[i+1].m_topVec[1],
-			zeroAccSpatFrc[i+1].m_topVec[2],
+				b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
+				i+1,
+				zeroAccSpatFrc[i+1].m_topVec[0],
+				zeroAccSpatFrc[i+1].m_topVec[1],
+				zeroAccSpatFrc[i+1].m_topVec[2],
 
-			zeroAccSpatFrc[i+1].m_bottomVec[0],
-			zeroAccSpatFrc[i+1].m_bottomVec[1],
-			zeroAccSpatFrc[i+1].m_bottomVec[2]);
-		}
+				zeroAccSpatFrc[i+1].m_bottomVec[0],
+				zeroAccSpatFrc[i+1].m_bottomVec[1],
+				zeroAccSpatFrc[i+1].m_bottomVec[2]);
+			}
 #endif
-		//
-		//adding damping terms (only)
-		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()));
+			//
+			//adding damping terms (only)
+			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,21 +1115,23 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 		fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);
 
-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		if(!isLinkAndAllAncestorsKinematic(i))
 		{
-			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]);
+			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]);
 
-		spatAcc[i + 1] += spatCoriolisAcc[i];
+			spatAcc[i + 1] += spatCoriolisAcc[i];
 
-		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];
+			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,23 +1590,26 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 void btMultiBody::predictPositionsMultiDof(btScalar dt)
 {
     int num_links = getNumLinks();
-    // step position by adding dt * velocity
-    //btVector3 v = getBaseVel();
-    //m_basePos += dt * v;
-    //
-    btScalar *pBasePos;
-    btScalar *pBaseVel = &m_realBuf[3];  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
-    
-    // reset to current position
-    for (int i = 0; i < 3; ++i)
-    {
-        m_basePos_interpolate[i] = m_basePos[i];
-    }
-    pBasePos = m_basePos_interpolate;
+		if(!isBaseKinematic())
+		{
+      // step position by adding dt * velocity
+      //btVector3 v = getBaseVel();
+      //m_basePos += dt * v;
+      //
+      btScalar *pBasePos;
+      btScalar *pBaseVel = &m_realBuf[3];  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
     
-    pBasePos[0] += dt * pBaseVel[0];
-    pBasePos[1] += dt * pBaseVel[1];
-    pBasePos[2] += dt * pBaseVel[2];
+    	// reset to current position
+    	for (int i = 0; i < 3; ++i)
+    	{
+    	    m_basePos_interpolate[i] = m_basePos[i];
+    	}
+    	pBasePos = m_basePos_interpolate;
+    	
+    	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,26 +1660,29 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
     
     //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
     //
-    btScalar *pBaseQuat;
-
-    // reset to current orientation
-    for (int i = 0; i < 4; ++i)
-    {
-        m_baseQuat_interpolate[i] = m_baseQuat[i];
-    }
-    pBaseQuat = m_baseQuat_interpolate;
+		if(!isBaseKinematic())
+		{
+        btScalar *pBaseQuat;
 
-    btScalar *pBaseOmega = &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
-    //
-    btQuaternion baseQuat;
-    baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
-    btVector3 baseOmega;
-    baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
-    pQuatUpdateFun(baseOmega, baseQuat, true, dt);
-    pBaseQuat[0] = baseQuat.x();
-    pBaseQuat[1] = baseQuat.y();
-    pBaseQuat[2] = baseQuat.z();
-    pBaseQuat[3] = baseQuat.w();
+        // reset to current orientation
+        for (int i = 0; i < 4; ++i)
+        {
+            m_baseQuat_interpolate[i] = m_baseQuat[i];
+        }
+        pBaseQuat = m_baseQuat_interpolate;
+
+        btScalar *pBaseOmega = &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
+        //
+        btQuaternion baseQuat;
+        baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+        btVector3 baseOmega;
+        baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+        pQuatUpdateFun(baseOmega, baseQuat, true, dt);
+        pBaseQuat[0] = baseQuat.x();
+        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,55 +1690,88 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
         btScalar *pJointPos;
         pJointPos = &m_links[i].m_jointPos_interpolate[0];
         
-        btScalar *pJointVel = getJointVelMultiDof(i);
-        
-        switch (m_links[i].m_jointType)
-        {
-            case btMultibodyLink::ePrismatic:
-            case btMultibodyLink::eRevolute:
-            {
-                //reset to current pos
-                pJointPos[0] = m_links[i].m_jointPos[0];
-                btScalar jointVel = pJointVel[0];
-                pJointPos[0] += dt * jointVel;
-                break;
-            }
-            case btMultibodyLink::eSpherical:
-            {
-                //reset to current pos
-
-                for (int j = 0; j < 4; ++j)
+        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[j] = m_links[i].m_jointPos[j];
+                    pJointPos[0] = m_links[i].m_jointPos[0];
+                    break;
                 }
-                
-                btVector3 jointVel;
-                jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
-                btQuaternion jointOri;
-                jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
-                pQuatUpdateFun(jointVel, jointOri, false, dt);
-                pJointPos[0] = jointOri.x();
-                pJointPos[1] = jointOri.y();
-                pJointPos[2] = jointOri.z();
-                pJointPos[3] = jointOri.w();
-                break;
-            }
-            case btMultibodyLink::ePlanar:
-            {
-                for (int j = 0; j < 3; ++j)
+                case btMultibodyLink::eSpherical:
                 {
-                    pJointPos[j] = m_links[i].m_jointPos[j];
+                    for (int j = 0; j < 4; ++j)
+                    {
+                        pJointPos[j] = m_links[i].m_jointPos[j];
+                    }
+                    break;
                 }
-                pJointPos[0] += dt * getJointVelMultiDof(i)[0];
-                
-                btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
-                btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
-                pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
-                pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
-                break;
+                case btMultibodyLink::ePlanar:
+                {
+                    for (int j = 0; j < 3; ++j)
+                    {
+                        pJointPos[j] = m_links[i].m_jointPos[j];
+                    }
+                    break;
+                }
+                default:
+                   break;
             }
-            default:
+        }
+        else
+        {
+            btScalar *pJointVel = getJointVelMultiDof(i); 
+
+            switch (m_links[i].m_jointType)
             {
+                case btMultibodyLink::ePrismatic:
+                case btMultibodyLink::eRevolute:
+                {
+                    //reset to current pos
+                    pJointPos[0] = m_links[i].m_jointPos[0];
+                    btScalar jointVel = pJointVel[0];
+                    pJointPos[0] += dt * jointVel;
+                    break;
+                }
+                case btMultibodyLink::eSpherical:
+                {
+                    //reset to current pos
+
+                    for (int j = 0; j < 4; ++j)
+                    {
+                        pJointPos[j] = m_links[i].m_jointPos[j];
+                    }
+                    
+                    btVector3 jointVel;
+                    jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+                    btQuaternion jointOri;
+                    jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+                    pQuatUpdateFun(jointVel, jointOri, false, dt);
+                    pJointPos[0] = jointOri.x();
+                    pJointPos[1] = jointOri.y();
+                    pJointPos[2] = jointOri.z();
+                    pJointPos[3] = jointOri.w();
+                    break;
+                }
+                case btMultibodyLink::ePlanar:
+                {
+                    for (int j = 0; j < 3; ++j)
+                    {
+                        pJointPos[j] = m_links[i].m_jointPos[j];
+                    }
+                    pJointPos[0] += dt * getJointVelMultiDof(i)[0];
+                    
+                    btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
+                    btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
+                    pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
+                    pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
+                    break;
+                }
+                default:
+                {
+                }
             }
         }
         
@@ -1703,16 +1782,19 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
 void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
 {
 	int num_links = getNumLinks();
-	// step position by adding dt * velocity
-	//btVector3 v = getBaseVel();
-	//m_basePos += dt * v;
-	//
-    btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
-    btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
-    
-	pBasePos[0] += dt * pBaseVel[0];
-	pBasePos[1] += dt * pBaseVel[1];
-	pBasePos[2] += dt * pBaseVel[2];
+	if(!isBaseKinematic())
+	{
+		// step position by adding dt * velocity
+		//btVector3 v = getBaseVel();
+		//m_basePos += dt * v;
+		//
+  	  btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
+  	  btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
+  	  
+		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,22 +1845,25 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 
 	//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
 	//
-    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)
-	//
-	btQuaternion baseQuat;
-	baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
-	btVector3 baseOmega;
-	baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
-	pQuatUpdateFun(baseOmega, baseQuat, true, dt);
-	pBaseQuat[0] = baseQuat.x();
-	pBaseQuat[1] = baseQuat.y();
-	pBaseQuat[2] = baseQuat.z();
-	pBaseQuat[3] = baseQuat.w();
-
-	//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(!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)
+		//
+		btQuaternion baseQuat;
+		baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+		btVector3 baseOmega;
+		baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+		pQuatUpdateFun(baseOmega, baseQuat, true, dt);
+		pBaseQuat[0] = baseQuat.x();
+		pBaseQuat[1] = baseQuat.y();
+		pBaseQuat[2] = baseQuat.z();
+		pBaseQuat[3] = baseQuat.w();
+
+		//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;
@@ -1788,48 +1873,51 @@ 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)
 	{
-        btScalar *pJointPos;
-        pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
-        
-		btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
-
-		switch (m_links[i].m_jointType)
+		if (!(m_links[i].m_collider && m_links[i].m_collider->isStaticOrKinematic()))
 		{
-			case btMultibodyLink::ePrismatic:
-			case btMultibodyLink::eRevolute:
-			{
-                //reset to current pos
-				btScalar jointVel = pJointVel[0];
-				pJointPos[0] += dt * jointVel;
-				break;
-			}
-			case btMultibodyLink::eSpherical:
-			{
-                //reset to current pos
-				btVector3 jointVel;
-				jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
-				btQuaternion jointOri;
-				jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
-				pQuatUpdateFun(jointVel, jointOri, false, dt);
-				pJointPos[0] = jointOri.x();
-				pJointPos[1] = jointOri.y();
-				pJointPos[2] = jointOri.z();
-				pJointPos[3] = jointOri.w();
-				break;
-			}
-			case btMultibodyLink::ePlanar:
+			btScalar *pJointPos;
+			pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
+		
+			btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
+
+			switch (m_links[i].m_jointType)
 			{
-				pJointPos[0] += dt * getJointVelMultiDof(i)[0];
+				case btMultibodyLink::ePrismatic:
+				case btMultibodyLink::eRevolute:
+				{
+    	            //reset to current pos
+					btScalar jointVel = pJointVel[0];
+					pJointPos[0] += dt * jointVel;
+					break;
+				}
+				case btMultibodyLink::eSpherical:
+				{
+    	            //reset to current pos
+					btVector3 jointVel;
+					jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+					btQuaternion jointOri;
+					jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+					pQuatUpdateFun(jointVel, jointOri, false, dt);
+					pJointPos[0] = jointOri.x();
+					pJointPos[1] = jointOri.y();
+					pJointPos[2] = jointOri.z();
+					pJointPos[3] = jointOri.w();
+					break;
+				}
+				case btMultibodyLink::ePlanar:
+				{
+					pJointPos[0] += dt * getJointVelMultiDof(i)[0];
 
-				btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
-				btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
-				pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
-				pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
+					btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
+					btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
+					pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
+					pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
 
-				break;
-			}
-			default:
-			{
+					break;
+				}
+				default:
+				{
+				}
 			}
 		}
 
@@ -2135,8 +2223,15 @@ void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObj
     world_to_local.resize(getNumLinks() + 1);
     local_origin.resize(getNumLinks() + 1);
     
-    world_to_local[0] = getInterpolateWorldToBaseRot();
-    local_origin[0] = getInterpolateBasePos();
+		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,7 +210,13 @@ public:
 	void setBasePos(const btVector3 &pos)
 	{
 		m_basePos = pos;
-        m_basePos_interpolate = pos;
+		if(!isBaseKinematic())
+			m_basePos_interpolate = pos;
+	}
+
+	void setInterpolateBasePos(const btVector3 &pos)
+	{
+		m_basePos_interpolate = pos;
 	}
 
 	void setBaseWorldTransform(const btTransform &tr)
@@ -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!?
-        m_baseQuat_interpolate = rot;
+		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

@@ -13,13 +13,12 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver
 	btDeformableMultiBodyConstraintSolver* m_deformableSolver;
 
 	DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver,
-		 btDispatcher* dispatcher)
-	: MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver)
+											  btDispatcher* dispatcher)
+		: MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver)
 	{
 	}
 
-
-	virtual void processConstraints(int islandId=-1)
+	virtual void processConstraints(int islandId = -1)
 	{
 		btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
 		btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0;
@@ -30,7 +29,7 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver
 		//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
 
 		m_deformableSolver->solveDeformableBodyGroup(bodies, m_bodies.size(), softBodies, m_softBodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
-		if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1))
+		if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics & 1))
 		{
 			m_deformableSolver->m_analyticsData.m_islandId = islandId;
 			m_islandAnalyticsData.push_back(m_solver->m_analyticsData);

thirdparty/bullet/BulletSoftBody/btCGProjection.h

@@ -22,85 +22,83 @@
 
 struct DeformableContactConstraint
 {
-    const btSoftBody::Node* m_node;
-    btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
-    btAlignedObjectArray<btVector3> m_total_normal_dv;
-    btAlignedObjectArray<btVector3> m_total_tangent_dv;
-    btAlignedObjectArray<bool> m_static;
-    btAlignedObjectArray<bool> m_can_be_dynamic;
-    
-    DeformableContactConstraint(const btSoftBody::RContact& rcontact): m_node(rcontact.m_node)
-    {
-        append(rcontact);
-    }
-    
-    DeformableContactConstraint(): m_node(NULL)
-    {
-        m_contact.push_back(NULL);
-    }
-    
-    void append(const btSoftBody::RContact& rcontact)
-    {
-        m_contact.push_back(&rcontact);
-        m_total_normal_dv.push_back(btVector3(0,0,0));
-        m_total_tangent_dv.push_back(btVector3(0,0,0));
-        m_static.push_back(false);
-        m_can_be_dynamic.push_back(true);
-    }
-
-    void replace(const btSoftBody::RContact& rcontact)
-    {
-        m_contact.clear();
-        m_total_normal_dv.clear();
-        m_total_tangent_dv.clear();
-        m_static.clear();
-        m_can_be_dynamic.clear();
-        append(rcontact);
-    }
-    
-    ~DeformableContactConstraint()
-    {
-    }
+	const btSoftBody::Node* m_node;
+	btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
+	btAlignedObjectArray<btVector3> m_total_normal_dv;
+	btAlignedObjectArray<btVector3> m_total_tangent_dv;
+	btAlignedObjectArray<bool> m_static;
+	btAlignedObjectArray<bool> m_can_be_dynamic;
+
+	DeformableContactConstraint(const btSoftBody::RContact& rcontact) : m_node(rcontact.m_node)
+	{
+		append(rcontact);
+	}
+
+	DeformableContactConstraint() : m_node(NULL)
+	{
+		m_contact.push_back(NULL);
+	}
+
+	void append(const btSoftBody::RContact& rcontact)
+	{
+		m_contact.push_back(&rcontact);
+		m_total_normal_dv.push_back(btVector3(0, 0, 0));
+		m_total_tangent_dv.push_back(btVector3(0, 0, 0));
+		m_static.push_back(false);
+		m_can_be_dynamic.push_back(true);
+	}
+
+	void replace(const btSoftBody::RContact& rcontact)
+	{
+		m_contact.clear();
+		m_total_normal_dv.clear();
+		m_total_tangent_dv.clear();
+		m_static.clear();
+		m_can_be_dynamic.clear();
+		append(rcontact);
+	}
+
+	~DeformableContactConstraint()
+	{
+	}
 };
 
 class btCGProjection
 {
 public:
-    typedef btAlignedObjectArray<btVector3> TVStack;
-    typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
-    typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
-    btAlignedObjectArray<btSoftBody *>& m_softBodies;
-    const btScalar& m_dt;
-    // map from node indices to node pointers
-    const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
-    
-    btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
-    : m_softBodies(softBodies)
-    , m_dt(dt)
-    {
-    }
-    
-    virtual ~btCGProjection()
-    {
-    }
-    
-    // apply the constraints
-    virtual void project(TVStack& x) = 0;
-    
-    virtual void setConstraints() = 0;
-    
-    // update the constraints
-    virtual btScalar update() = 0;
-    
-    virtual void reinitialize(bool nodeUpdated)
-    {
-    }
-    
-    virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
-    {
-        m_nodes = nodes;
-    }
-};
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
+	typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
+	btAlignedObjectArray<btSoftBody*>& m_softBodies;
+	const btScalar& m_dt;
+	// map from node indices to node pointers
+	const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
+
+	btCGProjection(btAlignedObjectArray<btSoftBody*>& softBodies, const btScalar& dt)
+		: m_softBodies(softBodies), m_dt(dt)
+	{
+	}
 
+	virtual ~btCGProjection()
+	{
+	}
+
+	// apply the constraints
+	virtual void project(TVStack& x) = 0;
+
+	virtual void setConstraints() = 0;
+
+	// update the constraints
+	virtual btScalar update() = 0;
+
+	virtual void reinitialize(bool nodeUpdated)
+	{
+	}
+
+	virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
+	{
+		m_nodes = nodes;
+	}
+};
 
 #endif /* btCGProjection_h */

thirdparty/bullet/BulletSoftBody/btConjugateGradient.h

@@ -15,144 +15,103 @@
 
 #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;
-    TVStack r,p,z,temp;
-    int max_iterations;
-    btScalar tolerance_squared;
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	typedef btKrylovSolver<MatrixX> Base;
+	TVStack r, p, z, temp;
+
 public:
-    btConjugateGradient(const int max_it_in)
-    : max_iterations(max_it_in)
-    {
-       tolerance_squared = 1e-5;
-    }
-    
-    virtual ~btConjugateGradient(){}
-    
-    // return the number of iterations taken
-    int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
-    {
-        BT_PROFILE("CGSolve");
-        btAssert(x.size() == b.size());
-        reinitialize(b);
-        // r = b - A * x --with assigned dof zeroed out
-        A.multiply(x, temp);
-        r = 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) {
-            if (verbose)
-            {
-                std::cout << "Iteration = 0" << std::endl;
-                std::cout << "Two norm of the residual = " << r_dot_z << std::endl;
-            }
-            return 0;
-        }
-        p = z;
-        btScalar r_dot_z_new = r_dot_z;
-        for (int k = 1; k <= max_iterations; k++) {
-            // temp = A*p
-            A.multiply(p, temp);
-            A.project(temp);
-            if (dot(p,temp) < SIMD_EPSILON)
-            {
-                if (verbose)
-                    std::cout << "Encountered negative direction in CG!" << std::endl;
-                if (k == 1)
-                {
-                    x = b;
-                }
-              return k;
-            }
-            // alpha = r^T * z / (p^T * A * p)
-            btScalar alpha = r_dot_z_new / dot(p, temp);
-            //  x += alpha * p;
-            multAndAddTo(alpha, p, x);
-            //  r -= alpha * temp;
-            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) {
-                if (verbose)
-                {
-                    std::cout << "ConjugateGradient iterations " << k << std::endl;
-                }
-                return k;
-            }
+	btConjugateGradient(const int max_it_in)
+		: btKrylovSolver<MatrixX>(max_it_in, SIMD_EPSILON)
+	{
+	}
+
+	virtual ~btConjugateGradient() {}
+
+	// return the number of iterations taken
+	int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
+	{
+		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 = this->sub(b, temp);
+		A.project(r);
+		// z = M^(-1) * r
+		A.precondition(r, z);
+		A.project(z);
+		btScalar r_dot_z = this->dot(z, r);
+		if (r_dot_z <= Base::m_tolerance * d0)
+		{
+			if (verbose)
+			{
+				std::cout << "Iteration = 0" << std::endl;
+				std::cout << "Two norm of the residual = " << r_dot_z << std::endl;
+			}
+			return 0;
+		}
+		p = z;
+		btScalar r_dot_z_new = r_dot_z;
+		for (int k = 1; k <= Base::m_maxIterations; k++)
+		{
+			// temp = A*p
+			A.multiply(p, temp);
+			A.project(temp);
+			if (this->dot(p, temp) < 0)
+			{
+				if (verbose)
+					std::cout << "Encountered negative direction in CG!" << std::endl;
+				if (k == 1)
+				{
+					x = b;
+				}
+				return k;
+			}
+			// alpha = r^T * z / (p^T * A * p)
+			btScalar alpha = r_dot_z_new / this->dot(p, temp);
+			//  x += alpha * p;
+			this->multAndAddTo(alpha, p, x);
+			//  r -= alpha * temp;
+			this->multAndAddTo(-alpha, temp, r);
+			// z = M^(-1) * r
+			A.precondition(r, z);
+			r_dot_z = r_dot_z_new;
+			r_dot_z_new = this->dot(r, z);
+			if (r_dot_z_new < Base::m_tolerance * d0)
+			{
+				if (verbose)
+				{
+					std::cout << "ConjugateGradient iterations " << k << " residual = " << r_dot_z_new << std::endl;
+				}
+				return k;
+			}
+
+			btScalar beta = r_dot_z_new / r_dot_z;
+			p = this->multAndAdd(beta, p, z);
+		}
+		if (verbose)
+		{
+			std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << " error = " << r_dot_z_new << std::endl;
+		}
+		return Base::m_maxIterations;
+	}
 
-            btScalar beta = r_dot_z_new/r_dot_z;
-            p = multAndAdd(beta, p, z);
-        }
-        if (verbose)
-        {
-            std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
-        }
-        return max_iterations;
-    }
-    
-    void reinitialize(const TVStack& b)
-    {
-        r.resize(b.size());
-        p.resize(b.size());
-        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;
-    }
+	void reinitialize(const TVStack& b)
+	{
+		r.resize(b.size());
+		p.resize(b.size());
+		z.resize(b.size());
+		temp.resize(b.size());
+	}
 };
 #endif /* btConjugateGradient_h */

thirdparty/bullet/BulletSoftBody/btConjugateResidual.h

@@ -15,174 +15,98 @@
 
 #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;
-    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;
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	typedef btKrylovSolver<MatrixX> Base;
+	TVStack r, p, z, temp_p, temp_r, best_x;
+	// temp_r = A*r
+	// temp_p = A*p
+	// z = M^(-1) * temp_p = M^(-1) * A * p
+	btScalar best_r;
+
 public:
-    btConjugateResidual(const int max_it_in)
-    : max_iterations(max_it_in)
-    {
-        tolerance_squared = 1e-2;
-    }
-    
-    virtual ~btConjugateResidual(){}
-    
-    // return the number of iterations taken
-    int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
-    {
-        BT_PROFILE("CRSolve");
-        btAssert(x.size() == b.size());
-        reinitialize(b);
-        // r = b - A * x --with assigned dof zeroed out
-        A.multiply(x, temp_r); // borrow temp_r here to store A*x
-        r = 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)
-            {
-                std::cout << "Iteration = 0" << std::endl;
-                std::cout << "Two norm of the residual = " << residual_norm << std::endl;
-            }
-            return 0;
-        }
-        p = r;
-        btScalar r_dot_Ar, r_dot_Ar_new;
-        // temp_p = A*p
-        A.multiply(p, temp_p);
-        // temp_r = A*r
-        temp_r = temp_p;
-        r_dot_Ar = dot(r, temp_r);
-        for (int k = 1; k <= max_iterations; 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);
-            //  x += alpha * p;
-            multAndAddTo(alpha, p, x);
-            //  r -= alpha * z;
-            multAndAddTo(-alpha, z, r);
-            btScalar norm_r = norm(r);
-            if (norm_r < best_r)
-            {
-                best_x = x;
-                best_r = norm_r;
-                if (norm_r < tolerance_squared) {
-                    if (verbose)
-                    {
-                        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);
-            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);
-            // temp_p = beta*temp_p + temp_r;
-            temp_p = multAndAdd(beta, temp_p, temp_r);
-        }
-        if (verbose)
-        {
-            std::cout << "ConjugateResidual max iterations reached " << max_iterations << std::endl;
-        }
-        x = best_x;
-        return max_iterations;
-    }
-    
-    void reinitialize(const TVStack& b)
-    {
-        r.resize(b.size());
-        p.resize(b.size());
-        z.resize(b.size());
-        temp_p.resize(b.size());
-        temp_r.resize(b.size());
-        best_x.resize(b.size());
-        best_r = SIMD_INFINITY;
-    }
-    
-    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;
-    }
+	btConjugateResidual(const int max_it_in)
+		: Base(max_it_in, 1e-8)
+	{
+	}
+
+	virtual ~btConjugateResidual() {}
+
+	// return the number of iterations taken
+	int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
+	{
+		BT_PROFILE("CRSolve");
+		btAssert(x.size() == b.size());
+		reinitialize(b);
+		// r = b - A * x --with assigned dof zeroed out
+		A.multiply(x, temp_r);  // borrow temp_r here to store A*x
+		r = this->sub(b, temp_r);
+		// z = M^(-1) * r
+		A.precondition(r, z);  // borrow z to store preconditioned r
+		r = z;
+		btScalar residual_norm = this->norm(r);
+		if (residual_norm <= Base::m_tolerance)
+		{
+			return 0;
+		}
+		p = r;
+		btScalar r_dot_Ar, r_dot_Ar_new;
+		// temp_p = A*p
+		A.multiply(p, temp_p);
+		// temp_r = A*r
+		temp_r = temp_p;
+		r_dot_Ar = this->dot(r, temp_r);
+		for (int k = 1; k <= Base::m_maxIterations; k++)
+		{
+			// z = M^(-1) * Ap
+			A.precondition(temp_p, z);
+			// alpha = r^T * A * r / (Ap)^T * M^-1 * Ap)
+			btScalar alpha = r_dot_Ar / this->dot(temp_p, z);
+			//  x += alpha * p;
+			this->multAndAddTo(alpha, p, x);
+			//  r -= alpha * z;
+			this->multAndAddTo(-alpha, z, r);
+			btScalar norm_r = this->norm(r);
+			if (norm_r < best_r)
+			{
+				best_x = x;
+				best_r = norm_r;
+				if (norm_r < Base::m_tolerance)
+				{
+					return k;
+				}
+			}
+			// temp_r = A * r;
+			A.multiply(r, temp_r);
+			r_dot_Ar_new = this->dot(r, temp_r);
+			btScalar beta = r_dot_Ar_new / r_dot_Ar;
+			r_dot_Ar = r_dot_Ar_new;
+			// p = beta*p + r;
+			p = this->multAndAdd(beta, p, r);
+			// temp_p = beta*temp_p + temp_r;
+			temp_p = this->multAndAdd(beta, temp_p, temp_r);
+		}
+		if (verbose)
+		{
+			std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl;
+		}
+		x = best_x;
+		return Base::m_maxIterations;
+	}
+
+	void reinitialize(const TVStack& b)
+	{
+		r.resize(b.size());
+		p.resize(b.size());
+		z.resize(b.size());
+		temp_p.resize(b.size());
+		temp_r.resize(b.size());
+		best_x.resize(b.size());
+		best_r = SIMD_INFINITY;
+	}
 };
 #endif /* btConjugateResidual_h */
-

thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.cpp

@@ -17,211 +17,283 @@
 #include "btPreconditioner.h"
 #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)
+btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v)
+	: 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);
-    m_preconditioner = m_KKTPreconditioner;
+	m_massPreconditioner = new MassPreconditioner(m_softBodies);
+	m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit);
+	m_preconditioner = m_KKTPreconditioner;
 }
 
 btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective()
 {
-    delete m_KKTPreconditioner;
-    delete m_massPreconditioner;
+	delete m_KKTPreconditioner;
+	delete m_massPreconditioner;
 }
 
 void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt)
 {
-    BT_PROFILE("reinitialize");
-    if (dt > 0)
-    {
-        setDt(dt);
-    }
-    if(nodeUpdated)
-    {
-        updateId();
-    }
-    for (int i = 0; i < m_lf.size(); ++i)
-    {
-        m_lf[i]->reinitialize(nodeUpdated);
-    }
-    m_projection.reinitialize(nodeUpdated);
-//    m_preconditioner->reinitialize(nodeUpdated);
+	BT_PROFILE("reinitialize");
+	if (dt > 0)
+	{
+		setDt(dt);
+	}
+	if (nodeUpdated)
+	{
+		updateId();
+	}
+	for (int i = 0; i < m_lf.size(); ++i)
+	{
+		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);
 }
 
 void btDeformableBackwardEulerObjective::setDt(btScalar dt)
 {
-    m_dt = dt;
+	m_dt = dt;
 }
 
 void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
 {
-    BT_PROFILE("multiply");
-    // add in the mass term
-    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];
-            b[counter] = (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im;
-            ++counter;
-        }
-    }
-
-    for (int i = 0; i < m_lf.size(); ++i)
-    {
-        // add damping matrix
-        m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
-        if (m_implicit)
-        {
-             m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b);
-        }
-    }
-    int offset = m_nodes.size();
-    for (int i = offset; i < b.size(); ++i)
-    {
-        b[i].setZero();
-    }
-    // add in the lagrange multiplier terms
-    
-    for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c)
-    {
-        // C^T * lambda
-        const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c];
-        for (int i = 0; i < lm.m_num_nodes; ++i)
-        {
-            for (int j = 0; j < lm.m_num_constraints; ++j)
-            {
-                b[lm.m_indices[i]] += x[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j];
-            }
-        }
-        // C * x
-        for (int d = 0; d < lm.m_num_constraints; ++d)
-        {
-            for (int i = 0; i < lm.m_num_nodes; ++i)
-            {
-                b[offset+c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]);
-            }
-        }
-    }
+	BT_PROFILE("multiply");
+	// add in the mass term
+	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];
+			b[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : x[counter] / node.m_im;
+			++counter;
+		}
+	}
+
+	for (int i = 0; i < m_lf.size(); ++i)
+	{
+		// add damping matrix
+		m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
+        // Always integrate picking force implicitly for stability.
+        if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE)
+		{
+			m_lf[i]->addScaledElasticForceDifferential(-m_dt * m_dt, x, b);
+		}
+	}
+	int offset = m_nodes.size();
+	for (int i = offset; i < b.size(); ++i)
+	{
+		b[i].setZero();
+	}
+	// add in the lagrange multiplier terms
+
+	for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c)
+	{
+		// C^T * lambda
+		const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c];
+		for (int i = 0; i < lm.m_num_nodes; ++i)
+		{
+			for (int j = 0; j < lm.m_num_constraints; ++j)
+			{
+				b[lm.m_indices[i]] += x[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
+			}
+		}
+		// C * x
+		for (int d = 0; d < lm.m_num_constraints; ++d)
+		{
+			for (int i = 0; i < lm.m_num_nodes; ++i)
+			{
+				b[offset + c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]);
+			}
+		}
+	}
 }
 
 void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
 {
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        for (int j = 0; j < psb->m_nodes.size(); ++j)
-        {
-            btSoftBody::Node& node = psb->m_nodes[j];
-            node.m_v = m_backupVelocity[node.index] + dv[node.index];
-        }
-    }
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			btSoftBody::Node& node = psb->m_nodes[j];
+			node.m_v = m_backupVelocity[node.index] + dv[node.index];
+		}
+	}
 }
 
 void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero)
 {
-    size_t counter = 0;
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        if (!psb->isActive())
-        {
-            counter += psb->m_nodes.size();
-            continue;
-        }
-        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)
-            force[i].setZero();
-    }
+	size_t counter = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			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)
+			force[i].setZero();
+	}
 }
 
-void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual)
+void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack& residual)
 {
-    BT_PROFILE("computeResidual");
-    // add implicit force
-    for (int i = 0; i < m_lf.size(); ++i)
-    {
-        if (m_implicit)
-        {
-            m_lf[i]->addScaledForces(dt, residual);
-        }
-        else
-        {
-            m_lf[i]->addScaledDampingForce(dt, residual);
-        }
-    }
-//    m_projection.project(residual);
+	BT_PROFILE("computeResidual");
+	// add implicit force
+	for (int i = 0; i < m_lf.size(); ++i)
+	{
+        // Always integrate picking force implicitly for stability.
+		if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE)
+		{
+			m_lf[i]->addScaledForces(dt, residual);
+		}
+		else
+		{
+			m_lf[i]->addScaledDampingForce(dt, residual);
+		}
+	}
+	//    m_projection.project(residual);
 }
 
 btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const
 {
-    btScalar mag = 0;
-    for (int i = 0; i < residual.size(); ++i)
-    {
-        mag += residual[i].length2();
-    }
-    return std::sqrt(mag);
+	btScalar mag = 0;
+	for (int i = 0; i < residual.size(); ++i)
+	{
+		mag += residual[i].length2();
+	}
+	return std::sqrt(mag);
 }
 
 btScalar btDeformableBackwardEulerObjective::totalEnergy(btScalar dt)
 {
-    btScalar e = 0;
-    for (int i = 0; i < m_lf.size(); ++i)
-    {
-        e += m_lf[i]->totalEnergy(dt);
-    }
-    return e;
+	btScalar e = 0;
+	for (int i = 0; i < m_lf.size(); ++i)
+	{
+		e += m_lf[i]->totalEnergy(dt);
+	}
+	return e;
 }
 
 void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
 {
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        m_softBodies[i]->advanceDeformation();
-    }
-    
-    for (int i = 0; i < m_lf.size(); ++i)
-    {
-        m_lf[i]->addScaledExplicitForce(m_dt, force);
-    }
-    applyForce(force, true);
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		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);
 }
 
 void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual)
 {
-    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)
-        {
-            dv[counter] = psb->m_nodes[j].m_im * residual[counter];
-            ++counter;
-        }
-    }
+	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)
+		{
+			dv[counter] = psb->m_nodes[j].m_im * residual[counter];
+			++counter;
+		}
+	}
 }
 
 //set constraints as projections
 void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal)
 {
-    m_projection.setConstraints(infoGlobal);
+	m_projection.setConstraints(infoGlobal);
 }
 
 void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r)
 {
-     m_projection.applyDynamicFriction(r);
+	m_projection.applyDynamicFriction(r);
 }

thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -31,143 +31,168 @@
 class btDeformableBackwardEulerObjective
 {
 public:
-    typedef btAlignedObjectArray<btVector3> TVStack;
-    btScalar m_dt;
-    btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
-    btAlignedObjectArray<btSoftBody *>& m_softBodies;
-    Preconditioner* m_preconditioner;
-    btDeformableContactProjection m_projection;
-    const TVStack& m_backupVelocity;
-    btAlignedObjectArray<btSoftBody::Node* > m_nodes;
-    bool m_implicit;
-    MassPreconditioner* m_massPreconditioner;
-    KKTPreconditioner* m_KKTPreconditioner;
-
-    btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
-    
-    virtual ~btDeformableBackwardEulerObjective();
-    
-    void initialize(){}
-    
-    // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
-    void computeResidual(btScalar dt, TVStack& residual);
-    
-    // add explicit force to the velocity
-    void applyExplicitForce(TVStack& force);
-    
-    // apply force to velocity and optionally reset the force to zero
-    void applyForce(TVStack& force, bool setZero);
-    
-    // compute the norm of the residual
-    btScalar computeNorm(const TVStack& residual) const;
-    
-    // compute one step of the solve (there is only one solve if the system is linear)
-    void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
-    
-    // perform A*x = b
-    void multiply(const TVStack& x, TVStack& b) const;
-    
-    // set initial guess for CG solve
-    void initialGuess(TVStack& dv, const TVStack& residual);
-    
-    // reset data structure and reset dt
-    void reinitialize(bool nodeUpdated, btScalar dt);
-    
-    void setDt(btScalar dt);
-    
-    // add friction force to residual
-    void applyDynamicFriction(TVStack& r);
-    
-    // add dv to velocity
-    void updateVelocity(const TVStack& dv);
-    
-    //set constraints as projections
-    void setConstraints(const btContactSolverInfo& infoGlobal);
-    
-    // update the projections and project the residual
-    void project(TVStack& r)
-    {
-        BT_PROFILE("project");
-        m_projection.project(r);
-    }
-    
-    // perform precondition M^(-1) x = b
-    void precondition(const TVStack& x, TVStack& b)
-    {
-        m_preconditioner->operator()(x,b);
-    }
-
-    // reindex all the vertices 
-    virtual void updateId()
-    {
-        size_t node_id = 0;
-        size_t face_id = 0;
-        m_nodes.clear();
-        for (int i = 0; i < m_softBodies.size(); ++i)
-        {
-            btSoftBody* psb = m_softBodies[i];
-            for (int j = 0; j < psb->m_nodes.size(); ++j)
-            {
-                psb->m_nodes[j].index = node_id;
-                m_nodes.push_back(&psb->m_nodes[j]);
-                ++node_id;
-            }
-            for (int j = 0; j < psb->m_faces.size(); ++j)
-            {
-                psb->m_faces[j].m_index = face_id;
-                ++face_id;
-            }
-        }
-    }
-    
-    const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const
-    {
-        return &m_nodes;
-    }
-    
-    void setImplicit(bool implicit)
-    {
-        m_implicit = implicit;
-    }
-
-    // Calculate the total potential energy in the system
-    btScalar totalEnergy(btScalar dt);
-    
-    void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec)
-    {
-        extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size());
-        for (int i = 0; i < vec.size(); ++i)
-        {
-            extended_vec[i] = vec[i];
-        }
-        int offset = vec.size();
-        for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
-        {
-            extended_vec[offset + i].setZero();
-        }
-    }
-    
-    void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual)
-    {
-        extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size());
-        for (int i = 0; i < residual.size(); ++i)
-        {
-            extended_residual[i] = residual[i];
-        }
-        int offset = residual.size();
-        for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
-        {
-            const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i];
-            extended_residual[offset + i].setZero();
-            for (int d = 0; d < lm.m_num_constraints; ++d)
-            {
-                for (int n = 0; n < lm.m_num_nodes; ++n)
-                {
-                    extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]);
-                }
-            }
-        }
-    }
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btScalar m_dt;
+	btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
+	btAlignedObjectArray<btSoftBody*>& m_softBodies;
+	Preconditioner* m_preconditioner;
+	btDeformableContactProjection m_projection;
+	const TVStack& m_backupVelocity;
+	btAlignedObjectArray<btSoftBody::Node*> m_nodes;
+	bool m_implicit;
+	MassPreconditioner* m_massPreconditioner;
+	KKTPreconditioner* m_KKTPreconditioner;
+
+	btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v);
+
+	virtual ~btDeformableBackwardEulerObjective();
+
+	void initialize() {}
+
+	// compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
+	void computeResidual(btScalar dt, TVStack& residual);
+
+	// add explicit force to the velocity
+	void applyExplicitForce(TVStack& force);
+
+	// apply force to velocity and optionally reset the force to zero
+	void applyForce(TVStack& force, bool setZero);
+
+	// compute the norm of the residual
+	btScalar computeNorm(const TVStack& residual) const;
+
+	// compute one step of the solve (there is only one solve if the system is linear)
+	void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
+
+	// perform A*x = b
+	void multiply(const TVStack& x, TVStack& b) const;
+
+	// set initial guess for CG solve
+	void initialGuess(TVStack& dv, const TVStack& residual);
+
+	// reset data structure and reset dt
+	void reinitialize(bool nodeUpdated, btScalar dt);
+
+	void setDt(btScalar dt);
+
+	// add friction force to residual
+	void applyDynamicFriction(TVStack& r);
+
+	// add dv to velocity
+	void updateVelocity(const TVStack& dv);
+
+	//set constraints as projections
+	void setConstraints(const btContactSolverInfo& infoGlobal);
+
+	// update the projections and project the residual
+	void project(TVStack& r)
+	{
+		BT_PROFILE("project");
+		m_projection.project(r);
+	}
+
+	// perform precondition M^(-1) x = b
+	void precondition(const TVStack& x, TVStack& b)
+	{
+		m_preconditioner->operator()(x, b);
+	}
+
+	// reindex all the vertices
+	virtual void updateId()
+	{
+		size_t node_id = 0;
+		size_t face_id = 0;
+		m_nodes.clear();
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				psb->m_nodes[j].index = node_id;
+				m_nodes.push_back(&psb->m_nodes[j]);
+				++node_id;
+			}
+			for (int j = 0; j < psb->m_faces.size(); ++j)
+			{
+				psb->m_faces[j].m_index = face_id;
+				++face_id;
+			}
+		}
+	}
+
+	const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const
+	{
+		return &m_nodes;
+	}
+
+	void setImplicit(bool implicit)
+	{
+		m_implicit = implicit;
+	}
+
+	// Calculate the total potential energy in the system
+	btScalar totalEnergy(btScalar dt);
+
+	void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec)
+	{
+		extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size());
+		for (int i = 0; i < vec.size(); ++i)
+		{
+			extended_vec[i] = vec[i];
+		}
+		int offset = vec.size();
+		for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
+		{
+			extended_vec[offset + i].setZero();
+		}
+	}
+
+	void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual)
+	{
+		extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size());
+		for (int i = 0; i < residual.size(); ++i)
+		{
+			extended_residual[i] = residual[i];
+		}
+		int offset = residual.size();
+		for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
+		{
+			const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i];
+			extended_residual[offset + i].setZero();
+			for (int d = 0; d < lm.m_num_constraints; ++d)
+			{
+				for (int n = 0; n < lm.m_num_nodes; ++n)
+				{
+					extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]);
+				}
+			}
+		}
+	}
+
+	void calculateContactForce(const TVStack& dv, const TVStack& rhs, TVStack& f)
+	{
+		size_t counter = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				f[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : dv[counter] / node.m_im;
+				++counter;
+			}
+		}
+		for (int i = 0; i < m_lf.size(); ++i)
+		{
+			// add damping matrix
+			m_lf[i]->addScaledDampingForceDifferential(-m_dt, dv, f);
+		}
+		counter = 0;
+		for (; counter < f.size(); ++counter)
+		{
+			f[counter] = rhs[counter] - f[counter];
+		}
+	}
 };
 
 #endif /* btBackwardEulerObjective_h */

thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp

@@ -18,468 +18,489 @@
 #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);
+	m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
 }
 
 btDeformableBodySolver::~btDeformableBodySolver()
 {
-    delete m_objective;
+	delete m_objective;
 }
 
 void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
 {
-    BT_PROFILE("solveDeformableConstraints");
-    if (!m_implicit)
-    {
-        m_objective->computeResidual(solverdt, m_residual);
-        m_objective->applyDynamicFriction(m_residual);
-        if (m_useProjection)
-        {
-            computeStep(m_dv, m_residual);
-        }
-        else
-        {
-            TVStack rhs, x;
-            m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs);
-            m_objective->addLagrangeMultiplier(m_dv, x);
-            m_objective->m_preconditioner->reinitialize(true);
-            computeStep(x, rhs);
-            for (int i = 0; i<m_dv.size(); ++i)
-            {
-                    m_dv[i] = x[i];
-            }
-        }
-        updateVelocity();
-    }
-    else
-    {
-        for (int i = 0; i < m_maxNewtonIterations; ++i)
-        {
-            updateState();
-            // add the inertia term in the residual
-            int counter = 0;
-            for (int k = 0; k < m_softBodies.size(); ++k)
-            {
-                btSoftBody* psb = m_softBodies[k];
-                for (int j = 0; j < psb->m_nodes.size(); ++j)
-                {
-                    if (psb->m_nodes[j].m_im > 0)
-                    {
-                        m_residual[counter] = (-1./psb->m_nodes[j].m_im) *  m_dv[counter];
-                    }
-                    ++counter;
-                }
-            }
-
-            m_objective->computeResidual(solverdt, m_residual);
-            if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0)
-            {
-                break;
-            }
-            // todo xuchenhan@: this really only needs to be calculated once
-            m_objective->applyDynamicFriction(m_residual);
-            if (m_lineSearch)
-            {
-                btScalar inner_product = computeDescentStep(m_ddv,m_residual);
-                btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8
-                btScalar scale = 2;
-                btScalar f0 = m_objective->totalEnergy(solverdt)+kineticEnergy(), f1, f2;
-                backupDv();
-                do {
-                    scale *= beta;
-                    if (scale < 1e-8) {
-                        return;
-                    }
-                    updateEnergy(scale);
-                    f1 = m_objective->totalEnergy(solverdt)+kineticEnergy();
-                    f2 = f0 - alpha * scale * inner_product;
-                } while (!(f1 < f2+SIMD_EPSILON)); // if anything here is nan then the search continues
-                revertDv();
-                updateDv(scale);
-            }
-            else
-            {
-                computeStep(m_ddv, m_residual);
-                updateDv();
-            }
-            for (int j = 0; j < m_numNodes; ++j)
-            {
-                m_ddv[j].setZero();
-                m_residual[j].setZero();
-            }
-        }
-        updateVelocity();
-    }
+	BT_PROFILE("solveDeformableConstraints");
+	if (!m_implicit)
+	{
+		m_objective->computeResidual(solverdt, m_residual);
+		m_objective->applyDynamicFriction(m_residual);
+		if (m_useProjection)
+		{
+			computeStep(m_dv, m_residual);
+		}
+		else
+		{
+			TVStack rhs, x;
+			m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs);
+			m_objective->addLagrangeMultiplier(m_dv, x);
+			m_objective->m_preconditioner->reinitialize(true);
+			computeStep(x, rhs);
+			for (int i = 0; i < m_dv.size(); ++i)
+			{
+				m_dv[i] = x[i];
+			}
+		}
+		updateVelocity();
+	}
+	else
+	{
+		for (int i = 0; i < m_maxNewtonIterations; ++i)
+		{
+			updateState();
+			// add the inertia term in the residual
+			int counter = 0;
+			for (int k = 0; k < m_softBodies.size(); ++k)
+			{
+				btSoftBody* psb = m_softBodies[k];
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					if (psb->m_nodes[j].m_im > 0)
+					{
+						m_residual[counter] = (-1. / psb->m_nodes[j].m_im) * m_dv[counter];
+					}
+					++counter;
+				}
+			}
+
+			m_objective->computeResidual(solverdt, m_residual);
+			if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0)
+			{
+				break;
+			}
+			// todo xuchenhan@: this really only needs to be calculated once
+			m_objective->applyDynamicFriction(m_residual);
+			if (m_lineSearch)
+			{
+				btScalar inner_product = computeDescentStep(m_ddv, m_residual);
+				btScalar alpha = 0.01, beta = 0.5;  // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8
+				btScalar scale = 2;
+				btScalar f0 = m_objective->totalEnergy(solverdt) + kineticEnergy(), f1, f2;
+				backupDv();
+				do
+				{
+					scale *= beta;
+					if (scale < 1e-8)
+					{
+						return;
+					}
+					updateEnergy(scale);
+					f1 = m_objective->totalEnergy(solverdt) + kineticEnergy();
+					f2 = f0 - alpha * scale * inner_product;
+				} while (!(f1 < f2 + SIMD_EPSILON));  // if anything here is nan then the search continues
+				revertDv();
+				updateDv(scale);
+			}
+			else
+			{
+				computeStep(m_ddv, m_residual);
+				updateDv();
+			}
+			for (int j = 0; j < m_numNodes; ++j)
+			{
+				m_ddv[j].setZero();
+				m_residual[j].setZero();
+			}
+		}
+		updateVelocity();
+	}
 }
 
 btScalar btDeformableBodySolver::kineticEnergy()
 {
-    btScalar ke = 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)
-        {
-            btSoftBody::Node& node = psb->m_nodes[j];
-            if (node.m_im > 0)
-            {
-                ke += m_dv[node.index].length2() * 0.5 / node.m_im;
-            }
-        }
-    }
-    return ke;
+	btScalar ke = 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)
+		{
+			btSoftBody::Node& node = psb->m_nodes[j];
+			if (node.m_im > 0)
+			{
+				ke += m_dv[node.index].length2() * 0.5 / node.m_im;
+			}
+		}
+	}
+	return ke;
 }
 
 void btDeformableBodySolver::backupDv()
 {
-    m_backup_dv.resize(m_dv.size());
-    for (int i = 0; i<m_backup_dv.size(); ++i)
-    {
-        m_backup_dv[i] = m_dv[i];
-    }
+	m_backup_dv.resize(m_dv.size());
+	for (int i = 0; i < m_backup_dv.size(); ++i)
+	{
+		m_backup_dv[i] = m_dv[i];
+	}
 }
 
 void btDeformableBodySolver::revertDv()
 {
-    for (int i = 0; i<m_backup_dv.size(); ++i)
-    {
-        m_dv[i] = m_backup_dv[i];
-    }
+	for (int i = 0; i < m_backup_dv.size(); ++i)
+	{
+		m_dv[i] = m_backup_dv[i];
+	}
 }
 
 void btDeformableBodySolver::updateEnergy(btScalar scale)
 {
-    for (int i = 0; i<m_dv.size(); ++i)
-    {
-        m_dv[i] = m_backup_dv[i] + scale * m_ddv[i];
-    }
-    updateState();
+	for (int i = 0; i < m_dv.size(); ++i)
+	{
+		m_dv[i] = m_backup_dv[i] + scale * m_ddv[i];
+	}
+	updateState();
 }
 
-
 btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose)
 {
-    m_cg.solve(*m_objective, ddv, residual, false);
-    btScalar inner_product = m_cg.dot(residual, m_ddv);
-    btScalar res_norm = m_objective->computeNorm(residual);
-    btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv);
-    if (inner_product < -tol)
-    {
-        if (verbose)
-        {
-            std::cout << "Looking backwards!" << std::endl;
-        }
-        for (int i = 0; i < m_ddv.size();++i)
-        {
-            m_ddv[i] = -m_ddv[i];
-        }
-        inner_product = -inner_product;
-    }
-    else if (std::abs(inner_product) < tol)
-    {
-        if (verbose)
-        {
-            std::cout << "Gradient Descent!" << std::endl;
-        }
-        btScalar scale = m_objective->computeNorm(m_ddv) / res_norm;
-        for (int i = 0; i < m_ddv.size();++i)
-        {
-            m_ddv[i] = scale * residual[i];
-        }
-        inner_product = scale * res_norm * res_norm;
-    }
-    return inner_product;
+	m_cg.solve(*m_objective, ddv, residual, false);
+	btScalar inner_product = m_cg.dot(residual, m_ddv);
+	btScalar res_norm = m_objective->computeNorm(residual);
+	btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv);
+	if (inner_product < -tol)
+	{
+		if (verbose)
+		{
+			std::cout << "Looking backwards!" << std::endl;
+		}
+		for (int i = 0; i < m_ddv.size(); ++i)
+		{
+			m_ddv[i] = -m_ddv[i];
+		}
+		inner_product = -inner_product;
+	}
+	else if (std::abs(inner_product) < tol)
+	{
+		if (verbose)
+		{
+			std::cout << "Gradient Descent!" << std::endl;
+		}
+		btScalar scale = m_objective->computeNorm(m_ddv) / res_norm;
+		for (int i = 0; i < m_ddv.size(); ++i)
+		{
+			m_ddv[i] = scale * residual[i];
+		}
+		inner_product = scale * res_norm * res_norm;
+	}
+	return inner_product;
 }
 
 void btDeformableBodySolver::updateState()
 {
-    updateVelocity();
-    updateTempPosition();
+	updateVelocity();
+	updateTempPosition();
 }
 
 void btDeformableBodySolver::updateDv(btScalar scale)
 {
-    for (int i = 0; i < m_numNodes; ++i)
-    {
-        m_dv[i] += scale * m_ddv[i];
-    }
+	for (int i = 0; i < m_numNodes; ++i)
+	{
+		m_dv[i] += scale * m_ddv[i];
+	}
 }
 
 void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
 {
-    if (m_useProjection)
-        m_cg.solve(*m_objective, ddv, residual, false);
-    else
-        m_cr.solve(*m_objective, ddv, residual, false);
+	if (m_useProjection)
+		m_cg.solve(*m_objective, ddv, residual, false);
+	else
+		m_cr.solve(*m_objective, ddv, residual, false);
 }
 
-void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)
+void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt)
 {
-    m_softBodies.copyFromArray(softBodies);
-    bool nodeUpdated = updateNodes();
-    
-    if (nodeUpdated)
-    {
-        m_dv.resize(m_numNodes, btVector3(0,0,0));
-        m_ddv.resize(m_numNodes, btVector3(0,0,0));
-        m_residual.resize(m_numNodes, btVector3(0,0,0));
-        m_backupVelocity.resize(m_numNodes, btVector3(0,0,0));
-    }
-    
-    // need to setZero here as resize only set value for newly allocated items
-    for (int i = 0; i < m_numNodes; ++i)
-    {
-        m_dv[i].setZero();
-        m_ddv[i].setZero();
-        m_residual[i].setZero();
-    }
-    
-    m_dt = dt;
-    m_objective->reinitialize(nodeUpdated, dt);
-    updateSoftBodies();
-}
+	m_softBodies.copyFromArray(softBodies);
+	bool nodeUpdated = updateNodes();
 
-void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
-{
-    BT_PROFILE("setConstraint");
-    m_objective->setConstraints(infoGlobal);
+	if (nodeUpdated)
+	{
+		m_dv.resize(m_numNodes, btVector3(0, 0, 0));
+		m_ddv.resize(m_numNodes, btVector3(0, 0, 0));
+		m_residual.resize(m_numNodes, btVector3(0, 0, 0));
+		m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0));
+	}
+
+	// need to setZero here as resize only set value for newly allocated items
+	for (int i = 0; i < m_numNodes; ++i)
+	{
+		m_dv[i].setZero();
+		m_ddv[i].setZero();
+		m_residual[i].setZero();
+	}
+
+	if (dt > 0)
+	{
+		m_dt = dt;
+	}
+	m_objective->reinitialize(nodeUpdated, dt);
+	updateSoftBodies();
 }
 
-btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal)
+void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
 {
-    BT_PROFILE("solveContactConstraints");
-    btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies, infoGlobal);
-    return maxSquaredResidual;
+	BT_PROFILE("setConstraint");
+	m_objective->setConstraints(infoGlobal);
 }
 
-void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
+btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 {
-     m_objective->m_projection.splitImpulseSetup(infoGlobal);
+	BT_PROFILE("solveContactConstraints");
+	btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies, numDeformableBodies, infoGlobal);
+	return maxSquaredResidual;
 }
 
 void btDeformableBodySolver::updateVelocity()
 {
-    int counter = 0;
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        psb->m_maxSpeedSquared = 0;
-        if (!psb->isActive())
-        {
-            counter += psb->m_nodes.size();
-            continue;
-        }
-        for (int j = 0; j < psb->m_nodes.size(); ++j)
-        {
-            // set NaN to zero;
-            if (m_dv[counter] != m_dv[counter])
-            {
-                m_dv[counter].setZero();
-            }
-            psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter];
-            psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
-            ++counter;
-        }
-    }
+	int counter = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		psb->m_maxSpeedSquared = 0;
+		if (!psb->isActive())
+		{
+			counter += psb->m_nodes.size();
+			continue;
+		}
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			// set NaN to zero;
+			if (m_dv[counter] != m_dv[counter])
+			{
+				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;
+		}
+	}
 }
 
 void btDeformableBodySolver::updateTempPosition()
 {
-    int counter = 0;
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        if (!psb->isActive())
-        {
-            counter += psb->m_nodes.size();
-            continue;
-        }
-        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;
-            ++counter;
-        }
-        psb->updateDeformation();
-    }
+	int counter = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			counter += psb->m_nodes.size();
+			continue;
+		}
+		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_splitv);
+			++counter;
+		}
+		psb->updateDeformation();
+	}
 }
 
 void btDeformableBodySolver::backupVelocity()
 {
-    int 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)
-        {
-            m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
-        }
-    }
+	int 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)
+		{
+			m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
+		}
+	}
 }
 
 void btDeformableBodySolver::setupDeformableSolve(bool implicit)
 {
-    int counter = 0;
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        if (!psb->isActive())
-        {
-            counter += psb->m_nodes.size();
-            continue;
-        }
-        for (int j = 0; j < psb->m_nodes.size(); ++j)
-        {
-            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];
-                else
-                    m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn;
-                m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
-            }
-            else
-            {
-                m_dv[counter] =  psb->m_nodes[j].m_v - m_backupVelocity[counter];
-            }
-            psb->m_nodes[j].m_v = m_backupVelocity[counter];
-            ++counter;
-        }
-    }
+	int counter = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			counter += psb->m_nodes.size();
+			continue;
+		}
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			if (implicit)
+			{
+				// 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 + psb->m_nodes[j].m_splitv - m_backupVelocity[counter];
+			}
+			psb->m_nodes[j].m_v = m_backupVelocity[counter];
+			++counter;
+		}
+	}
 }
 
 void btDeformableBodySolver::revertVelocity()
 {
-    int 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)
-        {
-            psb->m_nodes[j].m_v = m_backupVelocity[counter++];
-        }
-    }
+	int 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)
+		{
+			psb->m_nodes[j].m_v = m_backupVelocity[counter++];
+		}
+	}
 }
 
 bool btDeformableBodySolver::updateNodes()
 {
-    int numNodes = 0;
-    for (int i = 0; i < m_softBodies.size(); ++i)
-        numNodes += m_softBodies[i]->m_nodes.size();
-    if (numNodes != m_numNodes)
-    {
-        m_numNodes = numNodes;
-        return true;
-    }
-    return false;
+	int numNodes = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+		numNodes += m_softBodies[i]->m_nodes.size();
+	if (numNodes != m_numNodes)
+	{
+		m_numNodes = numNodes;
+		return true;
+	}
+	return false;
 }
 
-
 void btDeformableBodySolver::predictMotion(btScalar solverdt)
 {
-    // apply explicit forces to velocity
-    m_objective->applyExplicitForce(m_residual);
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody *psb = m_softBodies[i];
-        
-        if (psb->isActive())
-        {
-            // predict motion for collision detection
-            predictDeformableMotion(psb, solverdt);
-        }
-    }
+	// apply explicit forces to velocity
+	if (m_implicit)
+	{
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + psb->m_nodes[j].m_v * solverdt;
+				}
+			}
+		}
+	}
+	m_objective->applyExplicitForce(m_residual);
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+
+		if (psb->isActive())
+		{
+			// predict motion for collision detection
+			predictDeformableMotion(psb, solverdt);
+		}
+	}
 }
 
 void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt)
 {
-    BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
-    int i, ni;
-    
-    /* Update                */
-    if (psb->m_bUpdateRtCst)
-    {
-        psb->m_bUpdateRtCst = false;
-        psb->updateConstants();
-        psb->m_fdbvt.clear();
-        if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD)
-        {
-            psb->initializeFaceTree();
-        }
-    }
-    
-    /* Prepare                */
-    psb->m_sst.sdt = dt * psb->m_cfg.timescale;
-    psb->m_sst.isdt = 1 / psb->m_sst.sdt;
-    psb->m_sst.velmrg = psb->m_sst.sdt * 3;
-    psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
-    psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
-    /* Bounds                */
-    psb->updateBounds();
-    
-    /* Integrate            */
-    // do not allow particles to move more than the bounding box size
-    btScalar max_v = (psb->m_bounds[1]-psb->m_bounds[0]).norm() / dt;
-    for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
-    {
-        btSoftBody::Node& n = psb->m_nodes[i];
-        // apply drag
-        n.m_v *= (1 - psb->m_cfg.drag);
-        // scale velocity back
-        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;
-    }
-
-    /* Nodes                */
-    psb->updateNodeTree(true, true);
-    if (!psb->m_fdbvt.empty())
-    {
-        psb->updateFaceTree(true, true);
-    }
-    /* Clear contacts */
-    psb->m_nodeRigidContacts.resize(0);
-    psb->m_faceRigidContacts.resize(0);
-    psb->m_faceNodeContacts.resize(0);
-    /* Optimize dbvt's        */
-//    psb->m_ndbvt.optimizeIncremental(1);
-//    psb->m_fdbvt.optimizeIncremental(1);
-}
+	BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
+	int i, ni;
+
+	/* Update                */
+	if (psb->m_bUpdateRtCst)
+	{
+		psb->m_bUpdateRtCst = false;
+		psb->updateConstants();
+		psb->m_fdbvt.clear();
+		if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD)
+		{
+			psb->initializeFaceTree();
+		}
+	}
 
+	/* Prepare                */
+	psb->m_sst.sdt = dt * psb->m_cfg.timescale;
+	psb->m_sst.isdt = 1 / psb->m_sst.sdt;
+	psb->m_sst.velmrg = psb->m_sst.sdt * 3;
+	psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
+	psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
+	/* Bounds                */
+	psb->updateBounds();
+
+	/* Integrate            */
+	// do not allow particles to move more than the bounding box size
+	btScalar max_v = (psb->m_bounds[1] - psb->m_bounds[0]).norm() / dt;
+	for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
+	{
+		btSoftBody::Node& n = psb->m_nodes[i];
+		// 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_splitv.setZero();
+		n.m_constrained = false;
+	}
+
+	/* Nodes                */
+	psb->updateNodeTree(true, true);
+	if (!psb->m_fdbvt.empty())
+	{
+		psb->updateFaceTree(true, true);
+	}
+	/* Clear contacts */
+	psb->m_nodeRigidContacts.resize(0);
+	psb->m_faceRigidContacts.resize(0);
+	psb->m_faceNodeContacts.resize(0);
+	/* Optimize dbvt's        */
+	//    psb->m_ndbvt.optimizeIncremental(1);
+	//    psb->m_fdbvt.optimizeIncremental(1);
+}
 
 void btDeformableBodySolver::updateSoftBodies()
 {
-    BT_PROFILE("updateSoftBodies");
-    for (int i = 0; i < m_softBodies.size(); i++)
-    {
-        btSoftBody *psb = (btSoftBody *)m_softBodies[i];
-        if (psb->isActive())
-        {
-            psb->updateNormals();
-        }
-    }
+	BT_PROFILE("updateSoftBodies");
+	for (int i = 0; i < m_softBodies.size(); i++)
+	{
+		btSoftBody* psb = (btSoftBody*)m_softBodies[i];
+		if (psb->isActive())
+		{
+			psb->updateNormals();
+		}
+	}
 }
 
 void btDeformableBodySolver::setImplicit(bool implicit)
 {
-    m_implicit = implicit;
-    m_objective->setImplicit(implicit);
+	m_implicit = implicit;
+	m_objective->setImplicit(implicit);
 }
 
 void btDeformableBodySolver::setLineSearch(bool lineSearch)
 {
-    m_lineSearch = lineSearch;
+	m_lineSearch = lineSearch;
 }

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"
@@ -30,133 +29,132 @@ class btDeformableMultiBodyDynamicsWorld;
 
 class btDeformableBodySolver : public btSoftBodySolver
 {
-    typedef btAlignedObjectArray<btVector3> TVStack;
+	typedef btAlignedObjectArray<btVector3> TVStack;
+
 protected:
-    int m_numNodes;                 // total number of deformable body nodes
-    TVStack m_dv;                   // v_{n+1} - v_n
-    TVStack m_backup_dv;            // backed up dv
-    TVStack m_ddv;                  // incremental dv
-    TVStack m_residual;             // rhs of the linear solve
-    btAlignedObjectArray<btSoftBody *> m_softBodies;  // all deformable bodies
-    TVStack m_backupVelocity;       // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit
-    btScalar m_dt;                  // dt
-    btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;  // CG solver
-    btConjugateResidual<btDeformableBackwardEulerObjective> m_cr;  // CR solver
-    bool m_implicit;                // use implicit scheme if true, explicit scheme if false
-    int m_maxNewtonIterations;      // max number of newton iterations
-    btScalar m_newtonTolerance;     // stop newton iterations if f(x) < m_newtonTolerance
-    bool m_lineSearch;              // If true, use newton's method with line search under implicit scheme
+	int m_numNodes;                                                // total number of deformable body nodes
+	TVStack m_dv;                                                  // v_{n+1} - v_n
+	TVStack m_backup_dv;                                           // backed up dv
+	TVStack m_ddv;                                                 // incremental dv
+	TVStack m_residual;                                            // rhs of the linear solve
+	btAlignedObjectArray<btSoftBody*> m_softBodies;                // all deformable bodies
+	TVStack m_backupVelocity;                                      // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit
+	btScalar m_dt;                                                 // dt
+	btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;  // CG solver
+	btConjugateResidual<btDeformableBackwardEulerObjective> m_cr;  // CR solver
+	bool m_implicit;                                               // use implicit scheme if true, explicit scheme if false
+	int m_maxNewtonIterations;                                     // max number of newton iterations
+	btScalar m_newtonTolerance;                                    // stop newton iterations if f(x) < m_newtonTolerance
+	bool m_lineSearch;                                             // If true, use newton's method with line search under implicit scheme
 public:
-    // handles data related to objective function
-    btDeformableBackwardEulerObjective* m_objective;
-    bool m_useProjection;
-    
-    btDeformableBodySolver();
-    
-    virtual ~btDeformableBodySolver();
-    
-    virtual SolverTypes getSolverType() const
-    {
-        return DEFORMABLE_SOLVER;
-    }
-
-    // update soft body normals
-    virtual void updateSoftBodies();
-    
-    virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
-    
-    // solve the momentum equation
-    virtual void solveDeformableConstraints(btScalar solverdt);
-    
-    // set up the position error in split impulse
-    void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
-
-    // resize/clear data structures
-    void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
-    
-    // set up contact constraints
-    void setConstraints(const btContactSolverInfo& infoGlobal);
-    
-    // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
-    virtual void predictMotion(btScalar solverdt);
-    
-    // move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^*
-    // x_{n+1}^* is stored in m_q
-    void predictDeformableMotion(btSoftBody* psb, btScalar dt);
-    
-    // save the current velocity to m_backupVelocity
-    void backupVelocity();
-    
-    // set m_dv and m_backupVelocity to desired value to prepare for momentum solve
-    void setupDeformableSolve(bool implicit);
-    
-    // set the current velocity to that backed up in m_backupVelocity
-    void revertVelocity();
-    
-    // set velocity to m_dv + m_backupVelocity
-    void updateVelocity();
-    
-    // update the node count
-    bool updateNodes();
-    
-    // calculate the change in dv resulting from the momentum solve
-    void computeStep(TVStack& ddv, const TVStack& residual);
-    
-    // calculate the change in dv resulting from the momentum solve when line search is turned on
-    btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose=false);
-
-    virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
-
-    // process collision between deformable and rigid
-    virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap)
-    {
-        softBody->defaultCollisionHandler(collisionObjectWrap);
-    }
-    
-    // process collision between deformable and deformable
-    virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) {
-        softBody->defaultCollisionHandler(otherSoftBody);
-    }
-
-    // If true, implicit time stepping scheme is used.
-    // Otherwise, explicit time stepping scheme is used
-    void setImplicit(bool implicit);
-    
-    // If true, newton's method with line search is used when implicit time stepping scheme is turned on
-    void setLineSearch(bool lineSearch);
-    
-    // set temporary position x^* = x_n + dt * v
-    // update the deformation gradient at position x^*
-    void updateState();
-    
-    // set dv = dv + scale * ddv
-    void updateDv(btScalar scale = 1);
-    
-    // set temporary position x^* = x_n + dt * v^*
-    void updateTempPosition();
-    
-    // save the current dv to m_backup_dv;
-    void backupDv();
-    
-    // set dv to the backed-up value
-    void revertDv();
-    
-    // set dv = dv + scale * ddv
-    // set v^* = v_n + dv
-    // set temporary position x^* = x_n + dt * v^*
-    // update the deformation gradient at position x^*
-    void updateEnergy(btScalar scale);
-    
-    // calculates the appropriately scaled kinetic energy in the system, which is
-    // 1/2 * dv^T * M * dv
-    // used in line search
-    btScalar kineticEnergy();
-    
-    // unused functions
-    virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
-    virtual void solveConstraints(btScalar dt){}
-    virtual bool checkInitialized(){return true;}
-    virtual void copyBackToSoftBodies(bool bMove = true) {}
+	// handles data related to objective function
+	btDeformableBackwardEulerObjective* m_objective;
+	bool m_useProjection;
+
+	btDeformableBodySolver();
+
+	virtual ~btDeformableBodySolver();
+
+	virtual SolverTypes getSolverType() const
+	{
+		return DEFORMABLE_SOLVER;
+	}
+
+	// update soft body normals
+	virtual void updateSoftBodies();
+
+	virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
+
+	// solve the momentum equation
+	virtual void solveDeformableConstraints(btScalar solverdt);
+
+	// resize/clear data structures
+	void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt);
+
+	// set up contact constraints
+	void setConstraints(const btContactSolverInfo& infoGlobal);
+
+	// add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
+	virtual void predictMotion(btScalar solverdt);
+
+	// move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^*
+	// x_{n+1}^* is stored in m_q
+	void predictDeformableMotion(btSoftBody* psb, btScalar dt);
+
+	// save the current velocity to m_backupVelocity
+	void backupVelocity();
+
+	// set m_dv and m_backupVelocity to desired value to prepare for momentum solve
+	void setupDeformableSolve(bool implicit);
+
+	// set the current velocity to that backed up in m_backupVelocity
+	void revertVelocity();
+
+	// set velocity to m_dv + m_backupVelocity
+	void updateVelocity();
+
+	// update the node count
+	bool updateNodes();
+
+	// calculate the change in dv resulting from the momentum solve
+	void computeStep(TVStack& ddv, const TVStack& residual);
+
+	// calculate the change in dv resulting from the momentum solve when line search is turned on
+	btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose = false);
+
+	virtual void copySoftBodyToVertexBuffer(const btSoftBody* const softBody, btVertexBufferDescriptor* vertexBuffer) {}
+
+	// process collision between deformable and rigid
+	virtual void processCollision(btSoftBody* softBody, const btCollisionObjectWrapper* collisionObjectWrap)
+	{
+		softBody->defaultCollisionHandler(collisionObjectWrap);
+	}
+
+	// process collision between deformable and deformable
+	virtual void processCollision(btSoftBody* softBody, btSoftBody* otherSoftBody)
+	{
+		softBody->defaultCollisionHandler(otherSoftBody);
+	}
+
+	// If true, implicit time stepping scheme is used.
+	// Otherwise, explicit time stepping scheme is used
+	void setImplicit(bool implicit);
+
+	// If true, newton's method with line search is used when implicit time stepping scheme is turned on
+	void setLineSearch(bool lineSearch);
+
+	// set temporary position x^* = x_n + dt * v
+	// update the deformation gradient at position x^*
+	void updateState();
+
+	// set dv = dv + scale * ddv
+	void updateDv(btScalar scale = 1);
+
+	// set temporary position x^* = x_n + dt * v^*
+	void updateTempPosition();
+
+	// save the current dv to m_backup_dv;
+	void backupDv();
+
+	// set dv to the backed-up value
+	void revertDv();
+
+	// set dv = dv + scale * ddv
+	// set v^* = v_n + dv
+	// set temporary position x^* = x_n + dt * v^*
+	// update the deformation gradient at position x^*
+	void updateEnergy(btScalar scale);
+
+	// calculates the appropriately scaled kinetic energy in the system, which is
+	// 1/2 * dv^T * M * dv
+	// used in line search
+	btScalar kineticEnergy();
+
+	// unused functions
+	virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {}
+	virtual void solveConstraints(btScalar dt) {}
+	virtual bool checkInitialized() { return true; }
+	virtual void copyBackToSoftBodies(bool bMove = true) {}
 };
 
 #endif /* btDeformableBodySolver_h */

thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp

@@ -16,387 +16,503 @@
 #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)
 {
 }
 
 btVector3 btDeformableNodeAnchorConstraint::getVa() const
 {
-    const btSoftBody::sCti& cti = m_anchor->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->getVelocityInLocalPoint(m_anchor->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_anchor->jacobianData_normal.m_jacobians[0];
-                const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0];
-                const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0];
-                const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
-                const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
-                // add in the normal component of the va
-                btScalar vel = 0.0;
-                for (int k = 0; k < ndof; ++k)
-                {
-                    vel += (local_v[k]+local_dv[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_v[k]+local_dv[k]) * J_t1[k];
-                }
-                va += m_anchor->t1 * vel;
-                vel = 0.0;
-                for (int k = 0; k < ndof; ++k)
-                {
-                    vel += (local_v[k]+local_dv[k]) * J_t2[k];
-                }
-                va += m_anchor->t2 * vel;
-            }
-        }
-    }
-    return va;
+	const btSoftBody::sCti& cti = m_anchor->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->getVelocityInLocalPoint(m_anchor->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_anchor->jacobianData_normal.m_jacobians[0];
+				const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0];
+				const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0];
+				const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
+				const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
+				// add in the normal component of the va
+				btScalar vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += (local_v[k] + local_dv[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_v[k] + local_dv[k]) * J_t1[k];
+				}
+				va += m_anchor->t1 * vel;
+				vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += (local_v[k] + local_dv[k]) * J_t2[k];
+				}
+				va += m_anchor->t2 * vel;
+			}
+		}
+	}
+	return va;
 }
 
 btScalar btDeformableNodeAnchorConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 {
-    const btSoftBody::sCti& cti = m_anchor->m_cti;
-    btVector3 va = getVa();
-    btVector3 vb = getVb();
-    btVector3 vr = (vb - va);
-    // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0
-    const btScalar dn = btDot(vr, vr);
-    // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
-    btScalar residualSquare = dn*dn;
-    btVector3 impulse = m_anchor->m_c0 * vr;
-    // apply impulse to deformable nodes involved and change their velocities
-    applyImpulse(impulse);
-    
-    // apply 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->applyImpulse(impulse, m_anchor->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_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
-            // apply normal component of the impulse
-            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
-            // apply tangential component of the impulse
-            const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
-            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1));
-            const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
-            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2));
-        }
-    }
-    return residualSquare;
+	const btSoftBody::sCti& cti = m_anchor->m_cti;
+	btVector3 va = getVa();
+	btVector3 vb = getVb();
+	btVector3 vr = (vb - va);
+	// + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0
+	const btScalar dn = btDot(vr, vr);
+	// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+	btScalar residualSquare = dn * dn;
+	btVector3 impulse = m_anchor->m_c0 * vr;
+	// apply impulse to deformable nodes involved and change their velocities
+	applyImpulse(impulse);
+
+	// apply 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->applyImpulse(impulse, m_anchor->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_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+			// apply normal component of the impulse
+			multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
+			// apply tangential component of the impulse
+			const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+			multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1));
+			const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+			multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2));
+		}
+	}
+	return residualSquare;
 }
 
 btVector3 btDeformableNodeAnchorConstraint::getVb() const
 {
-    return m_anchor->m_node->m_v;
+	return m_anchor->m_node->m_v;
 }
 
 void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
 {
-    btVector3 dv = impulse * m_anchor->m_c2;
-    m_anchor->m_node->m_v -= dv;
+	btVector3 dv = impulse * m_anchor->m_c2;
+	m_anchor->m_node->m_v -= dv;
 }
 
 /* ================   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_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_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;
+	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;
-    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->getVelocityInLocalPoint(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_v = multibodyLinkCol->m_multiBody->getVelocityVector();
-                const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
-                // add in the normal component of the va
-                btScalar vel = 0.0;
-                for (int k = 0; k < ndof; ++k)
-                {
-                    vel += (local_v[k]+local_dv[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_v[k]+local_dv[k]) * J_t1[k];
-                }
-                va += m_contact->t1 * vel;
-                vel = 0.0;
-                for (int k = 0; k < ndof; ++k)
-                {
-                    vel += (local_v[k]+local_dv[k]) * J_t2[k];
-                }
-                va += m_contact->t2 * vel;
-            }
-        }
-    }
-    return va;
+	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->getVelocityInLocalPoint(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_v = multibodyLinkCol->m_multiBody->getVelocityVector();
+				const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
+				// add in the normal component of the va
+				btScalar vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += (local_v[k] + local_dv[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_v[k] + local_dv[k]) * J_t1[k];
+				}
+				va += m_contact->t1 * vel;
+				vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += (local_v[k] + local_dv[k]) * J_t2[k];
+				}
+				va += m_contact->t2 * vel;
+			}
+		}
+	}
+	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;
-    // 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_tangent = impulse - impulse_normal;
-    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;
-
-    if (m_total_normal_dv.dot(cti.m_normal) < 0)
-    {
-        // separating in the normal direction
-        m_static = false;
-        m_total_tangent_dv = btVector3(0,0,0);
-        impulse_tangent.setZero();
-    }
-    else
-    {
-        if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm())
-        {
-            // dynamic friction
-            // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
-            m_static = false;
-            if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
-            {
-                m_total_tangent_dv = btVector3(0,0,0);
-            }
-            else
-            {
-                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);
-        }
-        else
-        {
-            // static friction
-            m_static = true;
-        }
-    }
-    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)
-    {
-        btRigidBody* rigidCol = 0;
-        rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
-        if (rigidCol)
-        {
-            rigidCol->applyImpulse(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->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
-            if (impulse_tangent.norm() > SIMD_EPSILON)
-            {
-                // apply tangential component of the impulse
-                const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
-                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
-                const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
-                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
-            }
-        }
-    }
-//    va = getVa();
-//    vb = getVb();
-//    vr = vb - va;
-//    btScalar dn1 = btDot(vr, cti.m_normal) / 150;
-//    m_penetration += dn1;
-    return residualSquare;
+	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_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
+	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_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;
+		impulse_tangent.setZero();
+	}
+	else
+	{
+		if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm())
+		{
+			// dynamic friction
+			// with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
+			m_static = false;
+			if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
+			{
+				m_total_tangent_dv = btVector3(0, 0, 0);
+			}
+			else
+			{
+				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 = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv);
+		}
+		else
+		{
+			// static friction
+			m_static = true;
+		}
+	}
+	impulse = impulse_normal + impulse_tangent;
+	// apply impulse to deformable nodes involved and change their velocities
+	applyImpulse(impulse);
+	// apply 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->applyImpulse(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->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
+			if (impulse_tangent.norm() > SIMD_EPSILON)
+			{
+				// apply tangential component of the impulse
+				const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+				multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
+				const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+				multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
+			}
+		}
+	}
+	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)
 {
 }
 
 btVector3 btDeformableNodeRigidContactConstraint::getVb() const
 {
-    return m_node->m_v;
+	return m_node->m_v;
 }
 
+btVector3 btDeformableNodeRigidContactConstraint::getSplitVb() const
+{
+	return m_node->m_splitv;
+}
 
 btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
 {
-    return m_total_normal_dv + m_total_tangent_dv;
+	return m_total_normal_dv + m_total_tangent_dv;
 }
 
 void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
 {
-    const btSoftBody::DeformableNodeRigidContact* contact = getContact();
-    btVector3 dv = impulse * contact->m_c2;
-    contact->m_node->m_v -= dv;
+	const btSoftBody::DeformableNodeRigidContact* contact = getContact();
+	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)
 {
 }
 
 btVector3 btDeformableFaceRigidContactConstraint::getVb() const
 {
-    const btSoftBody::DeformableFaceRigidContact* contact = getContact();
-    btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
-    return vb;
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
+	return vb;
 }
 
-
 btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
 {
-    btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
-    const btSoftBody::DeformableFaceRigidContact* contact = getContact();
-    if (m_face->m_n[0] == node)
-    {
-        return face_dv * contact->m_weights[0];
-    }
-    if (m_face->m_n[1] == node)
-    {
-        return face_dv * contact->m_weights[1];
-    }
-    btAssert(node == m_face->m_n[2]);
-    return face_dv * contact->m_weights[2];
+	btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	if (m_face->m_n[0] == node)
+	{
+		return face_dv * contact->m_weights[0];
+	}
+	if (m_face->m_n[1] == node)
+	{
+		return face_dv * contact->m_weights[1];
+	}
+	btAssert(node == m_face->m_n[2]);
+	return face_dv * contact->m_weights[2];
 }
 
 void btDeformableFaceRigidContactConstraint::applyImpulse(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_v;
-    btVector3& v1 = face->m_n[1]->m_v;
-    btVector3& v2 = face->m_n[2]->m_v;
-    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];
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	btVector3 dv = impulse * contact->m_c2;
+	btSoftBody::Face* face = contact->m_face;
+
+	btVector3& v0 = face->m_n[0]->m_v;
+	btVector3& v1 = face->m_n[1]->m_v;
+	btVector3& v2 = face->m_n[2]->m_v;
+	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];
 	if (m_useStrainLimiting)
 	{
-		btScalar relaxation = 1./btScalar(m_infoGlobal->m_numIterations);
-		btScalar m01 = (relaxation/(im0 + im1));
-		btScalar m02 = (relaxation/(im0 + im2));
-		btScalar m12 = (relaxation/(im1 + im2));
-		#ifdef USE_STRAIN_RATE_LIMITING
+		btScalar relaxation = 1. / btScalar(m_infoGlobal->m_numIterations);
+		btScalar m01 = (relaxation / (im0 + im1));
+		btScalar m02 = (relaxation / (im0 + im2));
+		btScalar m12 = (relaxation / (im1 + im2));
+#ifdef USE_STRAIN_RATE_LIMITING
 		// apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%.
 		btScalar p = 0.01;
 		btVector3& x0 = face->m_n[0]->m_x;
 		btVector3& x1 = face->m_n[1]->m_x;
 		btVector3& x2 = face->m_n[2]->m_x;
-		const btVector3 x_diff[3] = {x1-x0, x2-x0, x2-x1};
-		const btVector3 v_diff[3] = {v1-v0, v2-v0, v2-v1};
+		const btVector3 x_diff[3] = {x1 - x0, x2 - x0, x2 - x1};
+		const btVector3 v_diff[3] = {v1 - v0, v2 - v0, v2 - v1};
 		btVector3 u[3];
 		btScalar x_diff_dot_u, dn[3];
 		btScalar dt = m_infoGlobal->m_timeStep;
@@ -404,172 +520,201 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
 		{
 			btScalar x_diff_norm = x_diff[i].safeNorm();
 			btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm();
-			btScalar strainRate = x_diff_norm_new/x_diff_norm;
+			btScalar strainRate = x_diff_norm_new / x_diff_norm;
 			u[i] = v_diff[i];
 			u[i].safeNormalize();
-			if (x_diff_norm == 0 || (1-p <= strainRate && strainRate <= 1+p))
+			if (x_diff_norm == 0 || (1 - p <= strainRate && strainRate <= 1 + p))
 			{
 				dn[i] = 0;
 				continue;
 			}
 			x_diff_dot_u = btDot(x_diff[i], u[i]);
 			btScalar s;
-			if (1-p > strainRate)
+			if (1 - p > strainRate)
 			{
-				s = 1/dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p-2*p) * x_diff_norm * x_diff_norm));
+				s = 1 / dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p - 2 * p) * x_diff_norm * x_diff_norm));
 			}
 			else
 			{
-				s = 1/dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p+2*p) * x_diff_norm * x_diff_norm));
+				s = 1 / dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p + 2 * p) * x_diff_norm * x_diff_norm));
 			}
 			//		x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm();
 			//		strainRate = x_diff_norm_new/x_diff_norm;
 			dn[i] = s - v_diff[i].safeNorm();
 		}
-		btVector3 dv0 = im0 * (m01 * u[0]*(-dn[0]) + m02 * u[1]*-(dn[1]));
-		btVector3 dv1 = im1 * (m01 * u[0]*(dn[0]) + m12 * u[2]*(-dn[2]));
-		btVector3 dv2 = im2 * (m12 * u[2]*(dn[2]) + m02 * u[1]*(dn[1]));
-	#else
+		btVector3 dv0 = im0 * (m01 * u[0] * (-dn[0]) + m02 * u[1] * -(dn[1]));
+		btVector3 dv1 = im1 * (m01 * u[0] * (dn[0]) + m12 * u[2] * (-dn[2]));
+		btVector3 dv2 = im2 * (m12 * u[2] * (dn[2]) + m02 * u[1] * (dn[1]));
+#else
 		// apply strain limiting to prevent undamped modes
-		btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
-		btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
-		btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
-	#endif
+		btVector3 dv0 = im0 * (m01 * (v1 - v0) + m02 * (v2 - v0));
+		btVector3 dv1 = im1 * (m01 * (v0 - v1) + m12 * (v2 - v1));
+		btVector3 dv2 = im2 * (m12 * (v1 - v2) + m02 * (v0 - v2));
+#endif
 		v0 += dv0;
 		v1 += dv1;
 		v2 += dv2;
 	}
 }
 
+btVector3 btDeformableFaceRigidContactConstraint::getSplitVb() const
+{
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv) * contact->m_bary[2];
+	return vb;
+}
+
+void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
+{
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	btVector3 dv = impulse * contact->m_c2;
+	btSoftBody::Face* face = contact->m_face;
+	btVector3& v0 = face->m_n[0]->m_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();
+	m_total_normal_dv.setZero();
+	m_total_tangent_dv.setZero();
 }
 
 btVector3 btDeformableFaceNodeContactConstraint::getVa() const
 {
-    return m_node->m_v;
+	return m_node->m_v;
 }
 
 btVector3 btDeformableFaceNodeContactConstraint::getVb() const
 {
-    const btSoftBody::DeformableFaceNodeContact* contact = getContact();
-    btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
-    return vb;
+	const btSoftBody::DeformableFaceNodeContact* contact = getContact();
+	btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
+	return vb;
 }
 
 btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n) const
 {
-    btVector3 dv = m_total_normal_dv + m_total_tangent_dv;
-    if (n == m_node)
-        return dv;
-    const btSoftBody::DeformableFaceNodeContact* contact = getContact();
-    if (m_face->m_n[0] == n)
-    {
-        return dv * contact->m_weights[0];
-    }
-    if (m_face->m_n[1] == n)
-    {
-        return dv * contact->m_weights[1];
-    }
-    btAssert(n == m_face->m_n[2]);
-    return dv * contact->m_weights[2];
+	btVector3 dv = m_total_normal_dv + m_total_tangent_dv;
+	if (n == m_node)
+		return dv;
+	const btSoftBody::DeformableFaceNodeContact* contact = getContact();
+	if (m_face->m_n[0] == n)
+	{
+		return dv * contact->m_weights[0];
+	}
+	if (m_face->m_n[1] == n)
+	{
+		return dv * contact->m_weights[1];
+	}
+	btAssert(n == m_face->m_n[2]);
+	return dv * contact->m_weights[2];
 }
 
 btScalar btDeformableFaceNodeContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 {
-    btVector3 va = getVa();
-    btVector3 vb = getVb();
-    btVector3 vr = vb - va;
-    const btScalar dn = btDot(vr, m_contact->m_normal);
-    // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
-    btScalar residualSquare = dn*dn;
-    btVector3 impulse = m_contact->m_c0 * vr;
-    const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn);
-    btVector3 impulse_tangent = impulse - impulse_normal;
-    
-    btVector3 old_total_tangent_dv = m_total_tangent_dv;
-    // m_c2 is the inverse mass of the deformable node/face
-    if (m_node->m_im > 0)
-    {
-        m_total_normal_dv -= impulse_normal * m_node->m_im;
-        m_total_tangent_dv -= impulse_tangent * m_node->m_im;
-    }
-    else
-    {
-        m_total_normal_dv -= impulse_normal * m_contact->m_imf;
-        m_total_tangent_dv -= impulse_tangent *  m_contact->m_imf;
-    }
-    
-    if (m_total_normal_dv.dot(m_contact->m_normal) > 0)
-    {
-        // separating in the normal direction
-        m_static = false;
-        m_total_tangent_dv = btVector3(0,0,0);
-        impulse_tangent.setZero();
-    }
-    else
-    {
-        if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm())
-        {
-            // dynamic friction
-            // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
-            m_static = false;
-            if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
-            {
-                m_total_tangent_dv = btVector3(0,0,0);
-            }
-            else
-            {
-                m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction;
-            }
-            impulse_tangent = -btScalar(1)/m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv);
-        }
-        else
-        {
-            // static friction
-            m_static = true;
-        }
-    }
-    impulse = impulse_normal + impulse_tangent;
-    // apply impulse to deformable nodes involved and change their velocities
-    applyImpulse(impulse);
-    return residualSquare;
+	btVector3 va = getVa();
+	btVector3 vb = getVb();
+	btVector3 vr = vb - va;
+	const btScalar dn = btDot(vr, m_contact->m_normal);
+	// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+	btScalar residualSquare = dn * dn;
+	btVector3 impulse = m_contact->m_c0 * vr;
+	const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn);
+	btVector3 impulse_tangent = impulse - impulse_normal;
+
+	btVector3 old_total_tangent_dv = m_total_tangent_dv;
+	// m_c2 is the inverse mass of the deformable node/face
+	if (m_node->m_im > 0)
+	{
+		m_total_normal_dv -= impulse_normal * m_node->m_im;
+		m_total_tangent_dv -= impulse_tangent * m_node->m_im;
+	}
+	else
+	{
+		m_total_normal_dv -= impulse_normal * m_contact->m_imf;
+		m_total_tangent_dv -= impulse_tangent * m_contact->m_imf;
+	}
+
+	if (m_total_normal_dv.dot(m_contact->m_normal) > 0)
+	{
+		// separating in the normal direction
+		m_static = false;
+		m_total_tangent_dv = btVector3(0, 0, 0);
+		impulse_tangent.setZero();
+	}
+	else
+	{
+		if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm())
+		{
+			// dynamic friction
+			// with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
+			m_static = false;
+			if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
+			{
+				m_total_tangent_dv = btVector3(0, 0, 0);
+			}
+			else
+			{
+				m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction;
+			}
+			impulse_tangent = -btScalar(1) / m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv);
+		}
+		else
+		{
+			// static friction
+			m_static = true;
+		}
+	}
+	impulse = impulse_normal + impulse_tangent;
+	// apply impulse to deformable nodes involved and change their velocities
+	applyImpulse(impulse);
+	return residualSquare;
 }
 
 void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impulse)
 {
-    const btSoftBody::DeformableFaceNodeContact* contact = getContact();
-    btVector3 dva = impulse * contact->m_node->m_im;
-    btVector3 dvb = impulse * contact->m_imf;
-    if (contact->m_node->m_im > 0)
-    {
-        contact->m_node->m_v += dva;
-    }
-    
-    btSoftBody::Face* face = contact->m_face;
-    btVector3& v0 = face->m_n[0]->m_v;
-    btVector3& v1 = face->m_n[1]->m_v;
-    btVector3& v2 = face->m_n[2]->m_v;
-    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 -= dvb * contact->m_weights[0];
-    }
-    if (im1 > 0)
-    {
-        v1 -= dvb * contact->m_weights[1];
-    }
-    if (im2 > 0)
-    {
-        v2 -= dvb * contact->m_weights[2];
-    }
+	const btSoftBody::DeformableFaceNodeContact* contact = getContact();
+	btVector3 dva = impulse * contact->m_node->m_im;
+	btVector3 dvb = impulse * contact->m_imf;
+	if (contact->m_node->m_im > 0)
+	{
+		contact->m_node->m_v += dva;
+	}
+
+	btSoftBody::Face* face = contact->m_face;
+	btVector3& v0 = face->m_n[0]->m_v;
+	btVector3& v1 = face->m_n[1]->m_v;
+	btVector3& v2 = face->m_n[2]->m_v;
+	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 -= dvb * contact->m_weights[0];
+	}
+	if (im1 > 0)
+	{
+		v1 -= dvb * contact->m_weights[1];
+	}
+	if (im2 > 0)
+	{
+		v2 -= dvb * contact->m_weights[2];
+	}
 }

thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.h

@@ -21,51 +21,49 @@
 class btDeformableContactConstraint
 {
 public:
-    // True if the friction is static
-    // False if the friction is dynamic
-    bool m_static;
+	// True if the friction is static
+	// False if the friction is dynamic
+	bool m_static;
 	const btContactSolverInfo* m_infoGlobal;
 
 	// normal of the contact
 	btVector3 m_normal;
 
-	btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal)
+	btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal)
 	{
 	}
 
-	btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal)
+	btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal)
 	{
 	}
-	
-	btDeformableContactConstraint(){}
+
+	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)
 	{
 	}
 
-    virtual ~btDeformableContactConstraint(){}
-    
-    // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence
-    // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact
-    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0;
-    
-    // get the velocity of the object A in the contact
-    virtual btVector3 getVa() const = 0;
-    
-    // get the velocity of the object B in the contact
-    virtual btVector3 getVb() const = 0;
-    
-    // get the velocity change of the soft body node in the constraint
-    virtual btVector3 getDv(const btSoftBody::Node*) const = 0;
-    
-    // apply impulse to the soft body node and/or face involved
-    virtual void applyImpulse(const btVector3& impulse) = 0;
-    
-    // scale the penetration depth by erp
-    virtual void setPenetrationScale(btScalar scale) = 0;
+	virtual ~btDeformableContactConstraint() {}
+
+	// solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence
+	// the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact
+	virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0;
+
+	// get the velocity of the object A in the contact
+	virtual btVector3 getVa() const = 0;
+
+	// get the velocity of the object B in the contact
+	virtual btVector3 getVb() const = 0;
+
+	// get the velocity change of the soft body node in the constraint
+	virtual btVector3 getDv(const btSoftBody::Node*) const = 0;
+
+	// apply impulse to the soft body node and/or face involved
+	virtual void applyImpulse(const btVector3& impulse) = 0;
+
+	// scale the penetration depth by erp
+	virtual void setPenetrationScale(btScalar scale) = 0;
 };
 
 //
@@ -73,42 +71,41 @@ public:
 class btDeformableStaticConstraint : public btDeformableContactConstraint
 {
 public:
-    btSoftBody::Node* m_node;
-    
-    btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal)
-    {
-    }
-	btDeformableStaticConstraint(){}
-    btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
-    : m_node(other.m_node)
-    , btDeformableContactConstraint(other)
-    {
-    }
-    
-    virtual ~btDeformableStaticConstraint(){}
-    
-    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal)
-    {
-        return 0;
-    }
-
-    virtual btVector3 getVa() const
-    {
-        return btVector3(0,0,0);
-    }
-    
-    virtual btVector3 getVb() const
-    {
-        return btVector3(0,0,0);
-    }
-    
-    virtual btVector3 getDv(const btSoftBody::Node* n) const
-    {
-        return btVector3(0,0,0);
-    }
-    
-    virtual void applyImpulse(const btVector3& impulse){}
-    virtual void setPenetrationScale(btScalar scale){}
+	btSoftBody::Node* m_node;
+
+	btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal) : m_node(node), btDeformableContactConstraint(false, btVector3(0, 0, 0), infoGlobal)
+	{
+	}
+	btDeformableStaticConstraint() {}
+	btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
+		: m_node(other.m_node), btDeformableContactConstraint(other)
+	{
+	}
+
+	virtual ~btDeformableStaticConstraint() {}
+
+	virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal)
+	{
+		return 0;
+	}
+
+	virtual btVector3 getVa() const
+	{
+		return btVector3(0, 0, 0);
+	}
+
+	virtual btVector3 getVb() const
+	{
+		return btVector3(0, 0, 0);
+	}
+
+	virtual btVector3 getDv(const btSoftBody::Node* n) const
+	{
+		return btVector3(0, 0, 0);
+	}
+
+	virtual void applyImpulse(const btVector3& impulse) {}
+	virtual void setPenetrationScale(btScalar scale) {}
 };
 
 //
@@ -116,56 +113,67 @@ public:
 class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint
 {
 public:
-    const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
-	
-    btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal);
-    btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
-	btDeformableNodeAnchorConstraint(){}
-    virtual ~btDeformableNodeAnchorConstraint()
-    {
-    }
-    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
-
-    // object A is the rigid/multi body, and object B is the deformable node/face
-    virtual btVector3 getVa() const;
-    // get the velocity of the deformable node in contact
-    virtual btVector3 getVb() const;
-    virtual btVector3 getDv(const btSoftBody::Node* n) const
-    {
-        return btVector3(0,0,0);
-    }
-    virtual void applyImpulse(const btVector3& impulse);
-
-    virtual void setPenetrationScale(btScalar scale){}
-};
+	const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
 
+	btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal);
+	btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
+	btDeformableNodeAnchorConstraint() {}
+	virtual ~btDeformableNodeAnchorConstraint()
+	{
+	}
+	virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
+
+	// object A is the rigid/multi body, and object B is the deformable node/face
+	virtual btVector3 getVa() const;
+	// get the velocity of the deformable node in contact
+	virtual btVector3 getVb() const;
+	virtual btVector3 getDv(const btSoftBody::Node* n) const
+	{
+		return btVector3(0, 0, 0);
+	}
+	virtual void applyImpulse(const btVector3& impulse);
+
+	virtual void setPenetrationScale(btScalar scale) {}
+};
 
 //
 // Constraint between rigid/multi body and deformable objects
 class btDeformableRigidContactConstraint : public btDeformableContactConstraint
 {
 public:
-    btVector3 m_total_normal_dv;
-    btVector3 m_total_tangent_dv;
-    btScalar m_penetration;
-    const btSoftBody::DeformableRigidContact* m_contact;
-	
-    btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal);
-    btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other);
-	btDeformableRigidContactConstraint(){}
-    virtual ~btDeformableRigidContactConstraint()
-    {
-    }
-    
-    // object A is the rigid/multi body, and object B is the deformable node/face
-    virtual btVector3 getVa() const;
-    
-    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
-    
-    virtual void setPenetrationScale(btScalar scale)
-    {
-        m_penetration *= scale;
-    }
+	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);
+	btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other);
+	btDeformableRigidContactConstraint() {}
+	virtual ~btDeformableRigidContactConstraint()
+	{
+	}
+
+	// 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;
 };
 
 //
@@ -173,29 +181,34 @@ public:
 class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint
 {
 public:
-    // the deformable node in contact
-    btSoftBody::Node* m_node;
-	
-    btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal);
-    btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other);
-	btDeformableNodeRigidContactConstraint(){}
-    virtual ~btDeformableNodeRigidContactConstraint()
-    {
-    }
-    
-    // get the velocity of the deformable node in contact
-    virtual btVector3 getVb() const;
-    
-    // get the velocity change of the input soft body node in the constraint
-    virtual btVector3 getDv(const btSoftBody::Node*) const;
-    
-    // cast the contact to the desired type
-    const btSoftBody::DeformableNodeRigidContact* getContact() const
-    {
-        return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact);
-    }
-    
-    virtual void applyImpulse(const btVector3& impulse);
+	// the deformable node in contact
+	btSoftBody::Node* m_node;
+
+	btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal);
+	btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other);
+	btDeformableNodeRigidContactConstraint() {}
+	virtual ~btDeformableNodeRigidContactConstraint()
+	{
+	}
+
+	// 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;
+
+	// cast the contact to the desired type
+	const btSoftBody::DeformableNodeRigidContact* getContact() const
+	{
+		return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact);
+	}
+
+	virtual void applyImpulse(const btVector3& impulse);
+
+	virtual void applySplitImpulse(const btVector3& impulse);
 };
 
 //
@@ -203,28 +216,33 @@ public:
 class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint
 {
 public:
-    const btSoftBody::Face* m_face;
-    bool m_useStrainLimiting;
-    btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting);
-    btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
-    btDeformableFaceRigidContactConstraint(): m_useStrainLimiting(false) {}
-    virtual ~btDeformableFaceRigidContactConstraint()
-    {
-    }
-    
-    // get the velocity of the deformable face at the contact point
-    virtual btVector3 getVb() const;
-    
-    // get the velocity change of the input soft body node in the constraint
-    virtual btVector3 getDv(const btSoftBody::Node*) const;
-    
-    // cast the contact to the desired type
-    const btSoftBody::DeformableFaceRigidContact* getContact() const
-    {
-        return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact);
-    }
-    
-    virtual void applyImpulse(const btVector3& impulse);
+	btSoftBody::Face* m_face;
+	bool m_useStrainLimiting;
+	btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting);
+	btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
+	btDeformableFaceRigidContactConstraint() : m_useStrainLimiting(false) {}
+	virtual ~btDeformableFaceRigidContactConstraint()
+	{
+	}
+
+	// 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;
+
+	// cast the contact to the desired type
+	const btSoftBody::DeformableFaceRigidContact* getContact() const
+	{
+		return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact);
+	}
+
+	virtual void applyImpulse(const btVector3& impulse);
+
+	virtual void applySplitImpulse(const btVector3& impulse);
 };
 
 //
@@ -232,35 +250,35 @@ public:
 class btDeformableFaceNodeContactConstraint : public btDeformableContactConstraint
 {
 public:
-    btSoftBody::Node* m_node;
-    btSoftBody::Face* m_face;
-    const btSoftBody::DeformableFaceNodeContact* m_contact;
-    btVector3 m_total_normal_dv;
-    btVector3 m_total_tangent_dv;
-    
-    btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal);
-	btDeformableFaceNodeContactConstraint(){}
-    virtual ~btDeformableFaceNodeContactConstraint(){}
-    
-    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
-    
-    // get the velocity of the object A in the contact
-    virtual btVector3 getVa() const;
-    
-    // get the velocity of the object B in the contact
-    virtual btVector3 getVb() const;
-    
-    // get the velocity change of the input soft body node in the constraint
-    virtual btVector3 getDv(const btSoftBody::Node*) const;
-    
-    // cast the contact to the desired type
-    const btSoftBody::DeformableFaceNodeContact* getContact() const
-    {
-        return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact);
-    }
-    
-    virtual void applyImpulse(const btVector3& impulse);
-
-    virtual void setPenetrationScale(btScalar scale){}
+	btSoftBody::Node* m_node;
+	btSoftBody::Face* m_face;
+	const btSoftBody::DeformableFaceNodeContact* m_contact;
+	btVector3 m_total_normal_dv;
+	btVector3 m_total_tangent_dv;
+
+	btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal);
+	btDeformableFaceNodeContactConstraint() {}
+	virtual ~btDeformableFaceNodeContactConstraint() {}
+
+	virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
+
+	// get the velocity of the object A in the contact
+	virtual btVector3 getVa() const;
+
+	// get the velocity of the object B in the contact
+	virtual btVector3 getVb() const;
+
+	// get the velocity change of the input soft body node in the constraint
+	virtual btVector3 getDv(const btSoftBody::Node*) const;
+
+	// cast the contact to the desired type
+	const btSoftBody::DeformableFaceNodeContact* getContact() const
+	{
+		return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact);
+	}
+
+	virtual void applyImpulse(const btVector3& impulse);
+
+	virtual void setPenetrationScale(btScalar scale) {}
 };
 #endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */

thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp

@@ -17,7 +17,7 @@
 #include "btDeformableMultiBodyDynamicsWorld.h"
 #include <algorithm>
 #include <cmath>
-btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal)
+btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 {
 	btScalar residualSquare = 0;
 	for (int i = 0; i < numDeformableBodies; ++i)
@@ -58,27 +58,37 @@ 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)
-		{
-			btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
-			constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
-		}
-		// face constraints
-		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+		for (int j = 0; j < m_softBodies.size(); ++j)
 		{
-			btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
-			constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
+			btCollisionObject* psb = m_softBodies[j];
+			if (psb != deformableBodies[i])
+			{
+				continue;
+			}
+			for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
+			{
+				btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
+				residualSquare = btMax(residualSquare, localResidualSquare);
+			}
+			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)
-{  
+{
 	BT_PROFILE("setConstraints");
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
@@ -97,7 +107,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
 				m_staticConstraints[i].push_back(static_constraint);
 			}
 		}
-		
+
 		// set up deformable anchors
 		for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
 		{
@@ -111,7 +121,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
 			btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal);
 			m_nodeAnchorConstraints[i].push_back(constraint);
 		}
-		
+
 		// set Deformable Node vs. Rigid constraint
 		for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
 		{
@@ -122,17 +132,9 @@ 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);
-			}
+			m_nodeRigidConstraints[i].push_back(constraint);
 		}
-		
+
 		// set Deformable Face vs. Rigid constraint
 		for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
 		{
@@ -143,15 +145,7 @@ 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);
-			}
+			m_faceRigidConstraints[i].push_back(constraint);
 		}
 	}
 }
@@ -159,267 +153,269 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
 void btDeformableContactProjection::project(TVStack& x)
 {
 #ifndef USE_MGS
-    const int dim = 3;
-    for (int index = 0; index < m_projectionsDict.size(); ++index)
-    {
-        btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
-        size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
-        if (projectionDirs.size() >= dim)
-        {
-            // static node
-            x[i].setZero();
-            continue;
-        }
-        else if (projectionDirs.size() == 2)
-        {
-            btVector3 dir0 = projectionDirs[0];
-            btVector3 dir1 = projectionDirs[1];
-            btVector3 free_dir = btCross(dir0, dir1);
-            if (free_dir.safeNorm() < SIMD_EPSILON)
-            {
-                x[i] -= x[i].dot(dir0) * dir0;
-                x[i] -= x[i].dot(dir1) * dir1;
-            }
-            else
-            {
-                free_dir.normalize();
-                x[i] = x[i].dot(free_dir) * free_dir;
-            }
-        }
-        else
-        {
-            btAssert(projectionDirs.size() == 1);
-            btVector3 dir0 = projectionDirs[0];
-            x[i] -= x[i].dot(dir0) * dir0;
-        }
-    }
+	const int dim = 3;
+	for (int index = 0; index < m_projectionsDict.size(); ++index)
+	{
+		btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
+		size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
+		if (projectionDirs.size() >= dim)
+		{
+			// static node
+			x[i].setZero();
+			continue;
+		}
+		else if (projectionDirs.size() == 2)
+		{
+			btVector3 dir0 = projectionDirs[0];
+			btVector3 dir1 = projectionDirs[1];
+			btVector3 free_dir = btCross(dir0, dir1);
+			if (free_dir.safeNorm() < SIMD_EPSILON)
+			{
+				x[i] -= x[i].dot(dir0) * dir0;
+			}
+			else
+			{
+				free_dir.normalize();
+				x[i] = x[i].dot(free_dir) * free_dir;
+			}
+		}
+		else
+		{
+			btAssert(projectionDirs.size() == 1);
+			btVector3 dir0 = projectionDirs[0];
+			x[i] -= x[i].dot(dir0) * dir0;
+		}
+	}
 #else
-    btReducedVector p(x.size());
-    for (int i = 0; i < m_projections.size(); ++i)
-    {
-        p += (m_projections[i].dot(x) * m_projections[i]);
-    }
-    for (int i = 0; i < p.m_indices.size(); ++i)
-    {
-        x[p.m_indices[i]] -= p.m_vecs[i];
-    }
+	btReducedVector p(x.size());
+	for (int i = 0; i < m_projections.size(); ++i)
+	{
+		p += (m_projections[i].dot(x) * m_projections[i]);
+	}
+	for (int i = 0; i < p.m_indices.size(); ++i)
+	{
+		x[p.m_indices[i]] -= p.m_vecs[i];
+	}
 #endif
 }
 
 void btDeformableContactProjection::setProjection()
 {
 #ifndef USE_MGS
-    BT_PROFILE("btDeformableContactProjection::setProjection");
-    btAlignedObjectArray<btVector3> units;
-    units.push_back(btVector3(1,0,0));
-    units.push_back(btVector3(0,1,0));
-    units.push_back(btVector3(0,0,1));
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        if (!psb->isActive())
-        {
-            continue;
-        }
-        for (int j = 0; j < m_staticConstraints[i].size(); ++j)
-        {
-            int index = m_staticConstraints[i][j].m_node->index;
-            m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
-            if (m_projectionsDict.find(index) == NULL)
-            {
-                m_projectionsDict.insert(index, units);
-            }
-            else
-            {
-                btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-                for (int k = 0; k < 3; ++k)
-                {
-                    projections.push_back(units[k]);
-                }
-            }
-        }
-        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;
-            if (m_projectionsDict.find(index) == NULL)
-            {
-                m_projectionsDict.insert(index, units);
-            }
-            else
-            {
-                btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-                for (int k = 0; k < 3; ++k)
-                {
-                    projections.push_back(units[k]);
-                }
-            }
-        }
-        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;
-            if (m_nodeRigidConstraints[i][j].m_static)
-            {
-                if (m_projectionsDict.find(index) == NULL)
-                {
-                    m_projectionsDict.insert(index, units);
-                }
-                else
-                {
-                    btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-                    for (int k = 0; k < 3; ++k)
-                    {
-                        projections.push_back(units[k]);
-                    }
-                }
-            }
-            else
-            {
-                if (m_projectionsDict.find(index) == NULL)
-                {
-                    btAlignedObjectArray<btVector3> projections;
-                    projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
-                    m_projectionsDict.insert(index, projections);
-                }
-                else
-                {
-                    btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-                    projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
-                }
-            }
-        }
-        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;
-            for (int k = 0; k < 3; ++k)
-            {
-                face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
-            }
-            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)
-                {
-                    if (m_projectionsDict.find(index) == NULL)
-                    {
-                        m_projectionsDict.insert(index, units);
-                    }
-                    else
-                    {
-                        btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-                        for (int k = 0; k < 3; ++k)
-                        {
-                            projections.push_back(units[k]);
-                        }
-                    }
-                }
-                else
-                {
-                    if (m_projectionsDict.find(index) == NULL)
-                    {
-                        btAlignedObjectArray<btVector3> projections;
-                        projections.push_back(m_faceRigidConstraints[i][j].m_normal);
-                        m_projectionsDict.insert(index, projections);
-                    }
-                    else
-                    {
-                        btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-                        projections.push_back(m_faceRigidConstraints[i][j].m_normal);
-                    }
-                }
-            }
-        }
-    }
+	BT_PROFILE("btDeformableContactProjection::setProjection");
+	btAlignedObjectArray<btVector3> units;
+	units.push_back(btVector3(1, 0, 0));
+	units.push_back(btVector3(0, 1, 0));
+	units.push_back(btVector3(0, 0, 1));
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			continue;
+		}
+		for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+		{
+			int index = m_staticConstraints[i][j].m_node->index;
+			m_staticConstraints[i][j].m_node->m_constrained = true;
+			if (m_projectionsDict.find(index) == NULL)
+			{
+				m_projectionsDict.insert(index, units);
+			}
+			else
+			{
+				btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+				for (int k = 0; k < 3; ++k)
+				{
+					projections.push_back(units[k]);
+				}
+			}
+		}
+		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
+		{
+			int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
+			m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
+			if (m_projectionsDict.find(index) == NULL)
+			{
+				m_projectionsDict.insert(index, units);
+			}
+			else
+			{
+				btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+				for (int k = 0; k < 3; ++k)
+				{
+					projections.push_back(units[k]);
+				}
+			}
+		}
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+		{
+			int index = m_nodeRigidConstraints[i][j].m_node->index;
+			m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
+			if (m_nodeRigidConstraints[i][j].m_binding)
+			{
+				if (m_nodeRigidConstraints[i][j].m_static)
+				{
+					if (m_projectionsDict.find(index) == NULL)
+					{
+						m_projectionsDict.insert(index, units);
+					}
+					else
+					{
+						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+						for (int k = 0; k < 3; ++k)
+						{
+							projections.push_back(units[k]);
+						}
+					}
+				}
+				else
+				{
+					if (m_projectionsDict.find(index) == NULL)
+					{
+						btAlignedObjectArray<btVector3> projections;
+						projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
+						m_projectionsDict.insert(index, projections);
+					}
+					else
+					{
+						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+						projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
+					}
+				}
+			}
+		}
+		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+		{
+			const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
+			if (m_faceRigidConstraints[i][j].m_binding)
+			{
+				for (int k = 0; k < 3; ++k)
+				{
+					face->m_n[k]->m_constrained = true;
+				}
+			}
+			for (int k = 0; k < 3; ++k)
+			{
+				btSoftBody::Node* node = face->m_n[k];
+				int index = node->index;
+				if (m_faceRigidConstraints[i][j].m_static)
+				{
+					if (m_projectionsDict.find(index) == NULL)
+					{
+						m_projectionsDict.insert(index, units);
+					}
+					else
+					{
+						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+						for (int l = 0; l < 3; ++l)
+						{
+							projections.push_back(units[l]);
+						}
+					}
+				}
+				else
+				{
+					if (m_projectionsDict.find(index) == NULL)
+					{
+						btAlignedObjectArray<btVector3> projections;
+						projections.push_back(m_faceRigidConstraints[i][j].m_normal);
+						m_projectionsDict.insert(index, projections);
+					}
+					else
+					{
+						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+						projections.push_back(m_faceRigidConstraints[i][j].m_normal);
+					}
+				}
+			}
+		}
+	}
 #else
-    int dof = 0;
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        dof += m_softBodies[i]->m_nodes.size();
-    }
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        if (!psb->isActive())
-        {
-            continue;
-        }
-        for (int j = 0; j < m_staticConstraints[i].size(); ++j)
-        {
-            int index = m_staticConstraints[i][j].m_node->index;
-            m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
-            btAlignedObjectArray<int> indices;
-            btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3;
-            indices.push_back(index);
-            vecs1.push_back(btVector3(1,0,0));
-            vecs2.push_back(btVector3(0,1,0));
-            vecs3.push_back(btVector3(0,0,1));
-            m_projections.push_back(btReducedVector(dof, indices, vecs1));
-            m_projections.push_back(btReducedVector(dof, indices, vecs2));
-            m_projections.push_back(btReducedVector(dof, indices, vecs3));
-        }
-        
-        for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
-        {
-            int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
-            m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
-            btAlignedObjectArray<int> indices;
-            btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3;
-            indices.push_back(index);
-            vecs1.push_back(btVector3(1,0,0));
-            vecs2.push_back(btVector3(0,1,0));
-            vecs3.push_back(btVector3(0,0,1));
-            m_projections.push_back(btReducedVector(dof, indices, vecs1));
-            m_projections.push_back(btReducedVector(dof, indices, vecs2));
-            m_projections.push_back(btReducedVector(dof, indices, vecs3));
-        }
-        for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
-        {
-            int index = m_nodeRigidConstraints[i][j].m_node->index;
-            m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
-            btAlignedObjectArray<int> indices;
-            indices.push_back(index);
-            btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3;
-            if (m_nodeRigidConstraints[i][j].m_static)
-            {
-                vecs1.push_back(btVector3(1,0,0));
-                vecs2.push_back(btVector3(0,1,0));
-                vecs3.push_back(btVector3(0,0,1));
-                m_projections.push_back(btReducedVector(dof, indices, vecs1));
-                m_projections.push_back(btReducedVector(dof, indices, vecs2));
-                m_projections.push_back(btReducedVector(dof, indices, vecs3));
-            }
-            else
-            {
-                vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal);
-                m_projections.push_back(btReducedVector(dof, indices, vecs1));
-            }
-        }
-        for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
-        {
-            const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
+	int dof = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		dof += m_softBodies[i]->m_nodes.size();
+	}
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			continue;
+		}
+		for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+		{
+			int index = m_staticConstraints[i][j].m_node->index;
+			m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
+			btAlignedObjectArray<int> indices;
+			btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
+			indices.push_back(index);
+			vecs1.push_back(btVector3(1, 0, 0));
+			vecs2.push_back(btVector3(0, 1, 0));
+			vecs3.push_back(btVector3(0, 0, 1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs2));
+			m_projections.push_back(btReducedVector(dof, indices, vecs3));
+		}
+
+		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
+		{
+			int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
+			m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
+			btAlignedObjectArray<int> indices;
+			btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
+			indices.push_back(index);
+			vecs1.push_back(btVector3(1, 0, 0));
+			vecs2.push_back(btVector3(0, 1, 0));
+			vecs3.push_back(btVector3(0, 0, 1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs2));
+			m_projections.push_back(btReducedVector(dof, indices, vecs3));
+		}
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+		{
+			int index = m_nodeRigidConstraints[i][j].m_node->index;
+			m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
+			btAlignedObjectArray<int> indices;
+			indices.push_back(index);
+			btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
+			if (m_nodeRigidConstraints[i][j].m_static)
+			{
+				vecs1.push_back(btVector3(1, 0, 0));
+				vecs2.push_back(btVector3(0, 1, 0));
+				vecs3.push_back(btVector3(0, 0, 1));
+				m_projections.push_back(btReducedVector(dof, indices, vecs1));
+				m_projections.push_back(btReducedVector(dof, indices, vecs2));
+				m_projections.push_back(btReducedVector(dof, indices, vecs3));
+			}
+			else
+			{
+				vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal);
+				m_projections.push_back(btReducedVector(dof, indices, vecs1));
+			}
+		}
+		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+		{
+			const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
 			btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
-            btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
-            for (int k = 0; k < 3; ++k)
-            {
-                face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
-            }
+			btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
+			for (int k = 0; k < 3; ++k)
+			{
+				face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
+			}
 			if (m_faceRigidConstraints[i][j].m_static)
 			{
 				for (int l = 0; l < 3; ++l)
 				{
-					
 					btReducedVector rv(dof);
 					for (int k = 0; k < 3; ++k)
 					{
 						rv.m_indices.push_back(face->m_n[k]->index);
-						btVector3 v(0,0,0);
+						btVector3 v(0, 0, 0);
 						v[l] = bary[k];
 						rv.m_vecs.push_back(v);
-                        rv.sort();
+						rv.sort();
 					}
 					m_projections.push_back(rv);
 				}
@@ -431,121 +427,134 @@ void btDeformableContactProjection::setProjection()
 				{
 					rv.m_indices.push_back(face->m_n[k]->index);
 					rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal);
-                    rv.sort();
+					rv.sort();
 				}
 				m_projections.push_back(rv);
 			}
 		}
-    }
-    btModifiedGramSchmidt<btReducedVector> mgs(m_projections);
-    mgs.solve();
-    m_projections = mgs.m_out;
+	}
+	btModifiedGramSchmidt<btReducedVector> mgs(m_projections);
+	mgs.solve();
+	m_projections = mgs.m_out;
 #endif
 }
 
 void btDeformableContactProjection::checkConstraints(const TVStack& x)
 {
-    for (int i = 0; i < m_lagrangeMultipliers.size(); ++i)
-    {
-        btVector3 d(0,0,0);
-        const LagrangeMultiplier& lm = m_lagrangeMultipliers[i];
-        for (int j = 0; j < lm.m_num_constraints; ++j)
-        {
-            for (int k = 0; k < lm.m_num_nodes; ++k)
-            {
-                d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
-            }
-        }
-        printf("d = %f, %f, %f\n",d[0],d[1],d[2]);
-    }
+	for (int i = 0; i < m_lagrangeMultipliers.size(); ++i)
+	{
+		btVector3 d(0, 0, 0);
+		const LagrangeMultiplier& lm = m_lagrangeMultipliers[i];
+		for (int j = 0; j < lm.m_num_constraints; ++j)
+		{
+			for (int k = 0; k < lm.m_num_nodes; ++k)
+			{
+				d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
+			}
+		}
+		//		printf("d = %f, %f, %f\n", d[0], d[1], d[2]);
+		//        printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]);
+	}
 }
 
 void btDeformableContactProjection::setLagrangeMultiplier()
 {
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        if (!psb->isActive())
-        {
-            continue;
-        }
-        for (int j = 0; j < m_staticConstraints[i].size(); ++j)
-        {
-            int index = m_staticConstraints[i][j].m_node->index;
-            m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
-            LagrangeMultiplier lm;
-            lm.m_num_nodes = 1;
-            lm.m_indices[0] = index;
-            lm.m_weights[0] = 1.0;
-            lm.m_num_constraints = 3;
-            lm.m_dirs[0] = btVector3(1,0,0);
-            lm.m_dirs[1] = btVector3(0,1,0);
-            lm.m_dirs[2] = btVector3(0,0,1);
-            m_lagrangeMultipliers.push_back(lm);
-        }
-        for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
-        {
-            int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
-            m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
-            LagrangeMultiplier lm;
-            lm.m_num_nodes = 1;
-            lm.m_indices[0] = index;
-            lm.m_weights[0] = 1.0;
-            lm.m_num_constraints = 3;
-            lm.m_dirs[0] = btVector3(1,0,0);
-            lm.m_dirs[1] = btVector3(0,1,0);
-            lm.m_dirs[2] = btVector3(0,0,1);
-            m_lagrangeMultipliers.push_back(lm);
-        }
-        for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
-        {
-            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;
-            LagrangeMultiplier lm;
-            lm.m_num_nodes = 1;
-            lm.m_indices[0] = index;
-            lm.m_weights[0] = 1.0;
-            if (m_nodeRigidConstraints[i][j].m_static)
-            {
-                lm.m_num_constraints = 3;
-                lm.m_dirs[0] = btVector3(1,0,0);
-                lm.m_dirs[1] = btVector3(0,1,0);
-                lm.m_dirs[2] = btVector3(0,0,1);
-            }
-            else
-            {
-                lm.m_num_constraints = 1;
-                lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal;
-            }
-            m_lagrangeMultipliers.push_back(lm);
-        }
-        for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
-        {
-            const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
-			
-            btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
-            btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			continue;
+		}
+		for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+		{
+			int index = m_staticConstraints[i][j].m_node->index;
+			m_staticConstraints[i][j].m_node->m_constrained = true;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 1;
+			lm.m_indices[0] = index;
+			lm.m_weights[0] = 1.0;
+			lm.m_num_constraints = 3;
+			lm.m_dirs[0] = btVector3(1, 0, 0);
+			lm.m_dirs[1] = btVector3(0, 1, 0);
+			lm.m_dirs[2] = btVector3(0, 0, 1);
+			m_lagrangeMultipliers.push_back(lm);
+		}
+		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
+		{
+			int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
+			m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 1;
+			lm.m_indices[0] = index;
+			lm.m_weights[0] = 1.0;
+			lm.m_num_constraints = 3;
+			lm.m_dirs[0] = btVector3(1, 0, 0);
+			lm.m_dirs[1] = btVector3(0, 1, 0);
+			lm.m_dirs[2] = btVector3(0, 0, 1);
+			m_lagrangeMultipliers.push_back(lm);
+		}
+
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+		{
+			if (!m_nodeRigidConstraints[i][j].m_binding)
+			{
+				continue;
+			}
+			int index = m_nodeRigidConstraints[i][j].m_node->index;
+			m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 1;
+			lm.m_indices[0] = index;
+			lm.m_weights[0] = 1.0;
+			if (m_nodeRigidConstraints[i][j].m_static)
+			{
+				lm.m_num_constraints = 3;
+				lm.m_dirs[0] = btVector3(1, 0, 0);
+				lm.m_dirs[1] = btVector3(0, 1, 0);
+				lm.m_dirs[2] = btVector3(0, 0, 1);
+			}
+			else
+			{
+				lm.m_num_constraints = 1;
+				lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal;
+			}
+			m_lagrangeMultipliers.push_back(lm);
+		}
+
+		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+		{
+			if (!m_faceRigidConstraints[i][j].m_binding)
+			{
+				continue;
+			}
+			btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
+
+			btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
 			LagrangeMultiplier lm;
 			lm.m_num_nodes = 3;
-			for (int k = 0; k<3; ++k)
+
+			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)
-            {
+			if (m_faceRigidConstraints[i][j].m_static)
+			{
+				face->m_pcontact[3] = 1;
 				lm.m_num_constraints = 3;
-				lm.m_dirs[0] = btVector3(1,0,0);
-				lm.m_dirs[1] = btVector3(0,1,0);
-				lm.m_dirs[2] = btVector3(0,0,1);
+				lm.m_dirs[0] = btVector3(1, 0, 0);
+				lm.m_dirs[1] = btVector3(0, 1, 0);
+				lm.m_dirs[2] = btVector3(0, 0, 1);
 			}
 			else
 			{
+				face->m_pcontact[3] = 0;
 				lm.m_num_constraints = 1;
 				lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
 			}
-            m_lagrangeMultipliers.push_back(lm);
+			m_lagrangeMultipliers.push_back(lm);
 		}
 	}
 }
@@ -562,7 +571,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 			if (node->m_im != 0)
 			{
 				int index = node->index;
-				f[index] += constraint.getDv(node)* (1./node->m_im);
+				f[index] += constraint.getDv(node) * (1. / node->m_im);
 			}
 		}
 		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
@@ -575,7 +584,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 				if (node->m_im != 0)
 				{
 					int index = node->index;
-					f[index] += constraint.getDv(node)* (1./node->m_im);
+					f[index] += constraint.getDv(node) * (1. / node->m_im);
 				}
 			}
 		}
@@ -587,7 +596,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 			if (node->m_im != 0)
 			{
 				int index = node->index;
-				f[index] += constraint.getDv(node)* (1./node->m_im);
+				f[index] += constraint.getDv(node) * (1. / node->m_im);
 			}
 			for (int k = 0; k < 3; ++k)
 			{
@@ -595,7 +604,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 				if (node->m_im != 0)
 				{
 					int index = node->index;
-					f[index] += constraint.getDv(node)* (1./node->m_im);
+					f[index] += constraint.getDv(node) * (1. / node->m_im);
 				}
 			}
 		}
@@ -612,9 +621,8 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 		m_nodeRigidConstraints.resize(N);
 		m_faceRigidConstraints.resize(N);
 		m_deformableConstraints.resize(N);
-		
 	}
-	for (int i = 0 ; i < N; ++i)
+	for (int i = 0; i < N; ++i)
 	{
 		m_staticConstraints[i].clear();
 		m_nodeAnchorConstraints[i].clear();
@@ -623,12 +631,9 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 		m_deformableConstraints[i].clear();
 	}
 #ifndef USE_MGS
-    m_projectionsDict.clear();
+	m_projectionsDict.clear();
 #else
-    m_projections.clear();
+	m_projections.clear();
 #endif
-    m_lagrangeMultipliers.clear();
+	m_lagrangeMultipliers.clear();
 }
-
-
-

thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h

@@ -27,31 +27,30 @@
 
 struct LagrangeMultiplier
 {
-    int m_num_constraints;        // Number of constraints
-    int m_num_nodes;              // Number of nodes in these constraints
-    btScalar m_weights[3];        // weights of the nodes involved, same size as m_num_nodes
-    btVector3 m_dirs[3];          // Constraint directions, same size of m_num_constraints;
-    int m_indices[3];             // indices of the nodes involved, same size as m_num_nodes;
+	int m_num_constraints;  // Number of constraints
+	int m_num_nodes;        // Number of nodes in these constraints
+	btScalar m_weights[3];  // weights of the nodes involved, same size as m_num_nodes
+	btVector3 m_dirs[3];    // Constraint directions, same size of m_num_constraints;
+	int m_indices[3];       // indices of the nodes involved, same size as m_num_nodes;
 };
 
-
 class btDeformableContactProjection
 {
 public:
-    typedef btAlignedObjectArray<btVector3> TVStack;
-    btAlignedObjectArray<btSoftBody *>& m_softBodies;
-	
-    // all constraints involving face
-    btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btAlignedObjectArray<btSoftBody*>& m_softBodies;
+
+	// all constraints involving face
+	btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
 #ifndef USE_MGS
-    // map from node index to projection directions
-    btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
+	// map from node index to projection directions
+	btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
 #else
-    btAlignedObjectArray<btReducedVector> m_projections;
+	btAlignedObjectArray<btReducedVector> m_projections;
 #endif
-    
-    btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers;
-    
+
+	btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers;
+
 	// map from node index to static constraint
 	btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints;
 	// map from node index to node rigid constraint
@@ -62,39 +61,39 @@ public:
 	btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceNodeContactConstraint> > m_deformableConstraints;
 	// map from node index to node anchor constraint
 	btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints;
-    
-    bool m_useStrainLimiting;
-    
-    btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
-    : m_softBodies(softBodies)
-    {
-    }
-    
-    virtual ~btDeformableContactProjection()
-    {
-    }
-    
-    // apply the constraints to the rhs of the linear solve
-    virtual void project(TVStack& x);
-    
-    // add friction force to the rhs of the linear solve
-    virtual void applyDynamicFriction(TVStack& f);
-    
-    // update and solve the constraints
-    virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
-    
-    // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
-    virtual void setConstraints(const btContactSolverInfo& infoGlobal);
-    
-    // Set up projections for each vertex by adding the projection direction to
-    virtual void setProjection();
-    
-    virtual void reinitialize(bool nodeUpdated);
-    
-    virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
-    
-    virtual void setLagrangeMultiplier();
-    
-    void checkConstraints(const TVStack& x);
+
+	bool m_useStrainLimiting;
+
+	btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies)
+		: m_softBodies(softBodies)
+	{
+	}
+
+	virtual ~btDeformableContactProjection()
+	{
+	}
+
+	// apply the constraints to the rhs of the linear solve
+	virtual void project(TVStack& x);
+
+	// add friction force to the rhs of the linear solve
+	virtual void applyDynamicFriction(TVStack& f);
+
+	// update and solve the constraints
+	virtual btScalar update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
+
+	// Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
+	virtual void setConstraints(const btContactSolverInfo& infoGlobal);
+
+	// Set up projections for each vertex by adding the projection direction to
+	virtual void setProjection();
+
+	virtual void reinitialize(bool nodeUpdated);
+
+	btScalar solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
+
+	virtual void setLagrangeMultiplier();
+
+	void checkConstraints(const TVStack& x);
 };
 #endif /* btDeformableContactProjection_h */

thirdparty/bullet/BulletSoftBody/btDeformableCorotatedForce.h

@@ -21,107 +21,104 @@
 
 static inline int PolarDecomposition(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
 {
-    static const btPolarDecomposition polar;
-    return polar.decompose(m, q, s);
+	static const btPolarDecomposition polar;
+	return polar.decompose(m, q, s);
 }
 
 class btDeformableCorotatedForce : public btDeformableLagrangianForce
 {
 public:
-    typedef btAlignedObjectArray<btVector3> TVStack;
-    btScalar m_mu, m_lambda;
-    btDeformableCorotatedForce(): m_mu(1), m_lambda(1)
-    {
-        
-    }
-    
-    btDeformableCorotatedForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda)
-    {
-    }
-    
-    virtual void addScaledForces(btScalar scale, TVStack& force)
-    {
-        addScaledElasticForce(scale, force);
-    }
-    
-    virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
-    {
-        addScaledElasticForce(scale, force);
-    }
-    
-    virtual void addScaledDampingForce(btScalar scale, TVStack& force)
-    {
-    }
-    
-    virtual void addScaledElasticForce(btScalar scale, TVStack& force)
-    {
-        int numNodes = getNumNodes();
-        btAssert(numNodes <= force.size());
-        btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
-        for (int i = 0; i < m_softBodies.size(); ++i)
-        {
-            btSoftBody* psb = m_softBodies[i];
-            for (int j = 0; j < psb->m_tetras.size(); ++j)
-            {
-                btSoftBody::Tetra& tetra = psb->m_tetras[j];
-                btMatrix3x3 P;
-                firstPiola(tetra.m_F,P);
-                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();
-                
-                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];
-                size_t id0 = node0->index;
-                size_t id1 = node1->index;
-                size_t id2 = node2->index;
-                size_t id3 = node3->index;
-                
-                // elastic force
-                // explicit elastic force
-                btScalar scale1 = scale * tetra.m_element_measure;
-                force[id0] -= scale1 * force_on_node0;
-                force[id1] -= scale1 * force_on_node123.getColumn(0);
-                force[id2] -= scale1 * force_on_node123.getColumn(1);
-                force[id3] -= scale1 * force_on_node123.getColumn(2);
-            }
-        }
-    }
-    
-    void firstPiola(const btMatrix3x3& F, btMatrix3x3& P)
-    {
-        // btMatrix3x3 JFinvT = F.adjoint();
-        btScalar J = F.determinant();
-        P =  F.adjoint().transpose() * (m_lambda * (J-1));
-        if (m_mu > SIMD_EPSILON)
-        {
-            btMatrix3x3 R,S;
-            if (J < 1024 * SIMD_EPSILON)
-                R.setIdentity();
-            else
-                PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd
-            /*https://fuchuyuan.github.io/research/svd/paper.pdf*/
-            P += (F-R) * 2 * m_mu;
-        }
-    }
-    
-    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
-    {
-    }
-    
-    virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
-    {
-    }
-    
-    virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){}
-    
-    virtual btDeformableLagrangianForceType getForceType()
-    {
-        return BT_COROTATED_FORCE;
-    }
-    
-};
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btScalar m_mu, m_lambda;
+	btDeformableCorotatedForce() : m_mu(1), m_lambda(1)
+	{
+	}
+
+	btDeformableCorotatedForce(btScalar mu, btScalar lambda) : m_mu(mu), m_lambda(lambda)
+	{
+	}
+
+	virtual void addScaledForces(btScalar scale, TVStack& force)
+	{
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+	{
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+	{
+	}
+
+	virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_tetras.size(); ++j)
+			{
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btMatrix3x3 P;
+				firstPiola(tetra.m_F, P);
+				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();
+
+				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];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
 
+				// elastic force
+				// explicit elastic force
+				btScalar scale1 = scale * tetra.m_element_measure;
+				force[id0] -= scale1 * force_on_node0;
+				force[id1] -= scale1 * force_on_node123.getColumn(0);
+				force[id2] -= scale1 * force_on_node123.getColumn(1);
+				force[id3] -= scale1 * force_on_node123.getColumn(2);
+			}
+		}
+	}
+
+	void firstPiola(const btMatrix3x3& F, btMatrix3x3& P)
+	{
+		// btMatrix3x3 JFinvT = F.adjoint();
+		btScalar J = F.determinant();
+		P = F.adjoint().transpose() * (m_lambda * (J - 1));
+		if (m_mu > SIMD_EPSILON)
+		{
+			btMatrix3x3 R, S;
+			if (J < 1024 * SIMD_EPSILON)
+				R.setIdentity();
+			else
+				PolarDecomposition(F, R, S);  // this QR is not robust, consider using implicit shift svd
+			/*https://fuchuyuan.github.io/research/svd/paper.pdf*/
+			P += (F - R) * 2 * m_mu;
+		}
+	}
+
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+	{
+	}
+
+	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+	{
+	}
+
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
+
+	virtual btDeformableLagrangianForceType getForceType()
+	{
+		return BT_COROTATED_FORCE;
+	}
+};
 
 #endif /* btCorotated_h */

thirdparty/bullet/BulletSoftBody/btDeformableGravityForce.h

@@ -21,87 +21,85 @@
 class btDeformableGravityForce : public btDeformableLagrangianForce
 {
 public:
-    typedef btAlignedObjectArray<btVector3> TVStack;
-    btVector3 m_gravity;
-    
-    btDeformableGravityForce(const btVector3& g) : m_gravity(g)
-    {
-    }
-    
-    virtual void addScaledForces(btScalar scale, TVStack& force)
-    {
-        addScaledGravityForce(scale, force);
-    }
-    
-    virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
-    {
-        addScaledGravityForce(scale, force);
-    }
-    
-    virtual void addScaledDampingForce(btScalar scale, TVStack& force)
-    {
-    }
-    
-    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
-    {
-    }
-    
-    virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
-    {
-    }
-    
-    virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){}
-    
-    virtual void addScaledGravityForce(btScalar scale, TVStack& force)
-    {
-        int numNodes = getNumNodes();
-        btAssert(numNodes <= force.size());
-        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_nodes.size(); ++j)
-            {
-                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;
-                force[id] += scaled_force;
-            }
-        }
-    }
-    
-    virtual btDeformableLagrangianForceType getForceType()
-    {
-        return BT_GRAVITY_FORCE;
-    }
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btVector3 m_gravity;
 
-    // the gravitational potential energy
-    virtual double totalEnergy(btScalar dt)
-    {
-        double e = 0;
-        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_nodes.size(); ++j)
-            {
-                const btSoftBody::Node& node = psb->m_nodes[j];
-                if (node.m_im > 0)
-                {
-                    e -= m_gravity.dot(node.m_q)/node.m_im;
-                }
-            }
-        }
-        return e;
-    }
-    
-    
+	btDeformableGravityForce(const btVector3& g) : m_gravity(g)
+	{
+	}
+
+	virtual void addScaledForces(btScalar scale, TVStack& force)
+	{
+		addScaledGravityForce(scale, force);
+	}
+
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+	{
+		addScaledGravityForce(scale, force);
+	}
+
+	virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+	{
+	}
+
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+	{
+	}
+
+	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+	{
+	}
+
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
+
+	virtual void addScaledGravityForce(btScalar scale, TVStack& force)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		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_nodes.size(); ++j)
+			{
+				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 * m_softBodies[i]->m_gravityFactor;
+				force[id] += scaled_force;
+			}
+		}
+	}
+
+	virtual btDeformableLagrangianForceType getForceType()
+	{
+		return BT_GRAVITY_FORCE;
+	}
+
+	// the gravitational potential energy
+	virtual double totalEnergy(btScalar dt)
+	{
+		double e = 0;
+		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_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				if (node.m_im > 0)
+				{
+					e -= m_gravity.dot(node.m_q) / node.m_im;
+				}
+			}
+		}
+		return e;
+	}
 };
 #endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */

thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h

@@ -22,352 +22,351 @@
 
 enum btDeformableLagrangianForceType
 {
-    BT_GRAVITY_FORCE = 1,
-    BT_MASSSPRING_FORCE = 2,
-    BT_COROTATED_FORCE = 3,
-    BT_NEOHOOKEAN_FORCE = 4,
-    BT_LINEAR_ELASTICITY_FORCE = 5,
-    BT_MOUSE_PICKING_FORCE = 6
+	BT_GRAVITY_FORCE = 1,
+	BT_MASSSPRING_FORCE = 2,
+	BT_COROTATED_FORCE = 3,
+	BT_NEOHOOKEAN_FORCE = 4,
+	BT_LINEAR_ELASTICITY_FORCE = 5,
+	BT_MOUSE_PICKING_FORCE = 6
 };
 
 static inline double randomDouble(double low, double high)
 {
-    return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
+	return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
 }
 
 class btDeformableLagrangianForce
 {
 public:
-    typedef btAlignedObjectArray<btVector3> TVStack;
-    btAlignedObjectArray<btSoftBody *> m_softBodies;
-    const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
-    
-    btDeformableLagrangianForce()
-    {
-    }
-    
-    virtual ~btDeformableLagrangianForce(){}
-    
-    // add all forces
-    virtual void addScaledForces(btScalar scale, TVStack& force) = 0;
-    
-    // add damping df
-    virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
-    
-    // build diagonal of A matrix
-    virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0;
-    
-    // add elastic df
-    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
-    
-    // add all forces that are explicit in explicit solve
-    virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
-    
-    // add all damping forces 
-    virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
-    
-    virtual btDeformableLagrangianForceType getForceType() = 0;
-    
-    virtual void reinitialize(bool nodeUpdated)
-    {
-    }
-    
-    // get number of nodes that have the force
-    virtual int getNumNodes()
-    {
-        int numNodes = 0;
-        for (int i = 0; i < m_softBodies.size(); ++i)
-        {
-            numNodes += m_softBodies[i]->m_nodes.size();
-        }
-        return numNodes;
-    }
-    
-    // add a soft body to be affected by the particular lagrangian force
-    virtual void addSoftBody(btSoftBody* psb)
-    {
-        m_softBodies.push_back(psb);
-    }
-    
-    virtual void removeSoftBody(btSoftBody* psb)
-    {
-        m_softBodies.remove(psb);
-    }
-    
-    virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
-    {
-        m_nodes = nodes;
-    }
-    
-     // Calculate the incremental deformable generated from the input dx
-    virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
-    {
-        btVector3 c1 = dx[id1] - dx[id0];
-        btVector3 c2 = dx[id2] - dx[id0];
-        btVector3 c3 = dx[id3] - dx[id0];
-        return btMatrix3x3(c1,c2,c3).transpose();
-    }
-    
-    // Calculate the incremental deformable generated from the current velocity
-    virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3)
-    {
-        btVector3 c1 = n1->m_v - n0->m_v;
-        btVector3 c2 = n2->m_v - n0->m_v;
-        btVector3 c3 = n3->m_v - n0->m_v;
-        return btMatrix3x3(c1,c2,c3).transpose();
-    }
-    
-    // test for addScaledElasticForce function
-    virtual void testDerivative()
-    {
-        for (int i = 0; i<m_softBodies.size();++i)
-        {
-            btSoftBody* psb = m_softBodies[i];
-            for (int j = 0; j < psb->m_nodes.size(); ++j)
-            {
-                psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
-            }
-            psb->updateDeformation();
-        }
-        
-        TVStack dx;
-        dx.resize(getNumNodes());
-        TVStack dphi_dx;
-        dphi_dx.resize(dx.size());
-        for (int i =0; i < dphi_dx.size();++i)
-        {
-            dphi_dx[i].setZero();
-        }
-        addScaledForces(-1, dphi_dx);
-        
-        // write down the current position
-        TVStack x;
-        x.resize(dx.size());
-        int 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)
-            {
-                x[counter] = psb->m_nodes[j].m_q;
-                counter++;
-            }
-        }
-        counter = 0;
-        
-        // populate dx with random vectors
-        for (int i = 0; i < dx.size(); ++i)
-        {
-            dx[i].setX(randomDouble(-1, 1));
-            dx[i].setY(randomDouble(-1, 1));
-            dx[i].setZ(randomDouble(-1, 1));
-        }
-        
-        btAlignedObjectArray<double> errors;
-        for (int it = 0; it < 10; ++it)
-        {
-            for (int i = 0; i < dx.size(); ++i)
-            {
-                dx[i] *= 0.5;
-            }
-            
-            // get dphi/dx * dx
-            double dphi = 0;
-            for (int i = 0; i < dx.size(); ++i)
-            {
-                dphi += dphi_dx[i].dot(dx[i]);
-            }
-            
-
-            for (int i = 0; i<m_softBodies.size();++i)
-            {
-                btSoftBody* psb = m_softBodies[i];
-                for (int j = 0; j < psb->m_nodes.size(); ++j)
-                {
-                    psb->m_nodes[j].m_q = x[counter] + dx[counter];
-                    counter++;
-                }
-                psb->updateDeformation();
-            }
-            counter = 0;
-            double f1 = totalElasticEnergy(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)
-                {
-                    psb->m_nodes[j].m_q = x[counter] - dx[counter];
-                    counter++;
-                }
-                psb->updateDeformation();
-            }
-            counter = 0;
-            
-            double f2 = totalElasticEnergy(0);
-            
-            //restore m_q
-            for (int i = 0; i<m_softBodies.size();++i)
-            {
-                btSoftBody* psb = m_softBodies[i];
-                for (int j = 0; j < psb->m_nodes.size(); ++j)
-                {
-                    psb->m_nodes[j].m_q = x[counter];
-                    counter++;
-                }
-                psb->updateDeformation();
-            }
-            counter = 0;
-            double error = f1-f2-2*dphi;
-            errors.push_back(error);
-            std::cout << "Iteration = " << it <<", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl;
-        }
-        for (int i = 1; i < errors.size(); ++i)
-        {
-            std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
-        }
-    }
-    
-    // test for addScaledElasticForce function
-    virtual void testHessian()
-    {
-        for (int i = 0; i<m_softBodies.size();++i)
-        {
-            btSoftBody* psb = m_softBodies[i];
-            for (int j = 0; j < psb->m_nodes.size(); ++j)
-            {
-                psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
-            }
-            psb->updateDeformation();
-        }
-        
-        
-        TVStack dx;
-        dx.resize(getNumNodes());
-        TVStack df;
-        df.resize(dx.size());
-        TVStack f1;
-        f1.resize(dx.size());
-        TVStack f2;
-        f2.resize(dx.size());
-        
-        
-        // write down the current position
-        TVStack x;
-        x.resize(dx.size());
-        int 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)
-            {
-                x[counter] = psb->m_nodes[j].m_q;
-                counter++;
-            }
-        }
-        counter = 0;
-        
-        // populate dx with random vectors
-        for (int i = 0; i < dx.size(); ++i)
-        {
-            dx[i].setX(randomDouble(-1, 1));
-            dx[i].setY(randomDouble(-1, 1));
-            dx[i].setZ(randomDouble(-1, 1));
-        }
-        
-        btAlignedObjectArray<double> errors;
-        for (int it = 0; it < 10; ++it)
-        {
-            for (int i = 0; i < dx.size(); ++i)
-            {
-                dx[i] *= 0.5;
-            }
-            
-            // get df
-            for (int i =0; i < df.size();++i)
-            {
-                df[i].setZero();
-                f1[i].setZero();
-                f2[i].setZero();
-            }
-
-            //set df
-            addScaledElasticForceDifferential(-1, dx, df);
-            
-            for (int i = 0; i<m_softBodies.size();++i)
-            {
-                btSoftBody* psb = m_softBodies[i];
-                for (int j = 0; j < psb->m_nodes.size(); ++j)
-                {
-                    psb->m_nodes[j].m_q = x[counter] + dx[counter];
-                    counter++;
-                }
-                psb->updateDeformation();
-            }
-            counter = 0;
-            
-            //set f1
-            addScaledForces(-1, f1);
-            
-            for (int i = 0; i<m_softBodies.size();++i)
-            {
-                btSoftBody* psb = m_softBodies[i];
-                for (int j = 0; j < psb->m_nodes.size(); ++j)
-                {
-                    psb->m_nodes[j].m_q = x[counter] - dx[counter];
-                    counter++;
-                }
-                psb->updateDeformation();
-            }
-            counter = 0;
-            
-            //set f2
-            addScaledForces(-1, f2);
-            
-            //restore m_q
-            for (int i = 0; i<m_softBodies.size();++i)
-            {
-                btSoftBody* psb = m_softBodies[i];
-                for (int j = 0; j < psb->m_nodes.size(); ++j)
-                {
-                    psb->m_nodes[j].m_q = x[counter];
-                    counter++;
-                }
-                psb->updateDeformation();
-            }
-            counter = 0;
-            double error = 0;
-            for (int i = 0; i < df.size();++i)
-            {
-                btVector3 error_vector = f1[i]-f2[i]-2*df[i];
-                error += error_vector.length2();
-            }
-            error = btSqrt(error);
-            errors.push_back(error);
-            std::cout << "Iteration = " << it << ", error = " << error << std::endl;
-        }
-        for (int i = 1; i < errors.size(); ++i)
-        {
-            std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
-        }
-    }
-    
-    //
-    virtual double totalElasticEnergy(btScalar dt)
-    {
-        return 0;
-    }
-    
-    //
-    virtual double totalDampingEnergy(btScalar dt)
-    {
-        return 0;
-    }
-    
-    // total Energy takes dt as input because certain energies depend on dt
-    virtual double totalEnergy(btScalar dt)
-    {
-        return totalElasticEnergy(dt) + totalDampingEnergy(dt);
-    }
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btAlignedObjectArray<btSoftBody*> m_softBodies;
+	const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
+
+	btDeformableLagrangianForce()
+	{
+	}
+
+	virtual ~btDeformableLagrangianForce() {}
+
+	// add all forces
+	virtual void addScaledForces(btScalar scale, TVStack& force) = 0;
+
+	// add damping df
+	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
+
+	// build diagonal of A matrix
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0;
+
+	// add elastic df
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
+
+	// add all forces that are explicit in explicit solve
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
+
+	// 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)
+	{
+	}
+
+	// get number of nodes that have the force
+	virtual int getNumNodes()
+	{
+		int numNodes = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			numNodes += m_softBodies[i]->m_nodes.size();
+		}
+		return numNodes;
+	}
+
+	// add a soft body to be affected by the particular lagrangian force
+	virtual void addSoftBody(btSoftBody* psb)
+	{
+		m_softBodies.push_back(psb);
+	}
+
+	virtual void removeSoftBody(btSoftBody* psb)
+	{
+		m_softBodies.remove(psb);
+	}
+
+	virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
+	{
+		m_nodes = nodes;
+	}
+
+	// Calculate the incremental deformable generated from the input dx
+	virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
+	{
+		btVector3 c1 = dx[id1] - dx[id0];
+		btVector3 c2 = dx[id2] - dx[id0];
+		btVector3 c3 = dx[id3] - dx[id0];
+		return btMatrix3x3(c1, c2, c3).transpose();
+	}
+
+	// Calculate the incremental deformable generated from the current velocity
+	virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3)
+	{
+		btVector3 c1 = n1->m_v - n0->m_v;
+		btVector3 c2 = n2->m_v - n0->m_v;
+		btVector3 c3 = n3->m_v - n0->m_v;
+		return btMatrix3x3(c1, c2, c3).transpose();
+	}
+
+	// test for addScaledElasticForce function
+	virtual void testDerivative()
+	{
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
+			}
+			psb->updateDeformation();
+		}
+
+		TVStack dx;
+		dx.resize(getNumNodes());
+		TVStack dphi_dx;
+		dphi_dx.resize(dx.size());
+		for (int i = 0; i < dphi_dx.size(); ++i)
+		{
+			dphi_dx[i].setZero();
+		}
+		addScaledForces(-1, dphi_dx);
+
+		// write down the current position
+		TVStack x;
+		x.resize(dx.size());
+		int 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)
+			{
+				x[counter] = psb->m_nodes[j].m_q;
+				counter++;
+			}
+		}
+		counter = 0;
+
+		// populate dx with random vectors
+		for (int i = 0; i < dx.size(); ++i)
+		{
+			dx[i].setX(randomDouble(-1, 1));
+			dx[i].setY(randomDouble(-1, 1));
+			dx[i].setZ(randomDouble(-1, 1));
+		}
+
+		btAlignedObjectArray<double> errors;
+		for (int it = 0; it < 10; ++it)
+		{
+			for (int i = 0; i < dx.size(); ++i)
+			{
+				dx[i] *= 0.5;
+			}
+
+			// get dphi/dx * dx
+			double dphi = 0;
+			for (int i = 0; i < dx.size(); ++i)
+			{
+				dphi += dphi_dx[i].dot(dx[i]);
+			}
+
+			for (int i = 0; i < m_softBodies.size(); ++i)
+			{
+				btSoftBody* psb = m_softBodies[i];
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = x[counter] + dx[counter];
+					counter++;
+				}
+				psb->updateDeformation();
+			}
+			counter = 0;
+			double f1 = totalElasticEnergy(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)
+				{
+					psb->m_nodes[j].m_q = x[counter] - dx[counter];
+					counter++;
+				}
+				psb->updateDeformation();
+			}
+			counter = 0;
+
+			double f2 = totalElasticEnergy(0);
+
+			//restore m_q
+			for (int i = 0; i < m_softBodies.size(); ++i)
+			{
+				btSoftBody* psb = m_softBodies[i];
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = x[counter];
+					counter++;
+				}
+				psb->updateDeformation();
+			}
+			counter = 0;
+			double error = f1 - f2 - 2 * dphi;
+			errors.push_back(error);
+			std::cout << "Iteration = " << it << ", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl;
+		}
+		for (int i = 1; i < errors.size(); ++i)
+		{
+			std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl;
+		}
+	}
+
+	// test for addScaledElasticForce function
+	virtual void testHessian()
+	{
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
+			}
+			psb->updateDeformation();
+		}
+
+		TVStack dx;
+		dx.resize(getNumNodes());
+		TVStack df;
+		df.resize(dx.size());
+		TVStack f1;
+		f1.resize(dx.size());
+		TVStack f2;
+		f2.resize(dx.size());
+
+		// write down the current position
+		TVStack x;
+		x.resize(dx.size());
+		int 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)
+			{
+				x[counter] = psb->m_nodes[j].m_q;
+				counter++;
+			}
+		}
+		counter = 0;
+
+		// populate dx with random vectors
+		for (int i = 0; i < dx.size(); ++i)
+		{
+			dx[i].setX(randomDouble(-1, 1));
+			dx[i].setY(randomDouble(-1, 1));
+			dx[i].setZ(randomDouble(-1, 1));
+		}
+
+		btAlignedObjectArray<double> errors;
+		for (int it = 0; it < 10; ++it)
+		{
+			for (int i = 0; i < dx.size(); ++i)
+			{
+				dx[i] *= 0.5;
+			}
+
+			// get df
+			for (int i = 0; i < df.size(); ++i)
+			{
+				df[i].setZero();
+				f1[i].setZero();
+				f2[i].setZero();
+			}
+
+			//set df
+			addScaledElasticForceDifferential(-1, dx, df);
+
+			for (int i = 0; i < m_softBodies.size(); ++i)
+			{
+				btSoftBody* psb = m_softBodies[i];
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = x[counter] + dx[counter];
+					counter++;
+				}
+				psb->updateDeformation();
+			}
+			counter = 0;
+
+			//set f1
+			addScaledForces(-1, f1);
+
+			for (int i = 0; i < m_softBodies.size(); ++i)
+			{
+				btSoftBody* psb = m_softBodies[i];
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = x[counter] - dx[counter];
+					counter++;
+				}
+				psb->updateDeformation();
+			}
+			counter = 0;
+
+			//set f2
+			addScaledForces(-1, f2);
+
+			//restore m_q
+			for (int i = 0; i < m_softBodies.size(); ++i)
+			{
+				btSoftBody* psb = m_softBodies[i];
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = x[counter];
+					counter++;
+				}
+				psb->updateDeformation();
+			}
+			counter = 0;
+			double error = 0;
+			for (int i = 0; i < df.size(); ++i)
+			{
+				btVector3 error_vector = f1[i] - f2[i] - 2 * df[i];
+				error += error_vector.length2();
+			}
+			error = btSqrt(error);
+			errors.push_back(error);
+			std::cout << "Iteration = " << it << ", error = " << error << std::endl;
+		}
+		for (int i = 1; i < errors.size(); ++i)
+		{
+			std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl;
+		}
+	}
+
+	//
+	virtual double totalElasticEnergy(btScalar dt)
+	{
+		return 0;
+	}
+
+	//
+	virtual double totalDampingEnergy(btScalar dt)
+	{
+		return 0;
+	}
+
+	// total Energy takes dt as input because certain energies depend on dt
+	virtual double totalEnergy(btScalar dt)
+	{
+		return totalElasticEnergy(dt) + totalDampingEnergy(dt);
+	}
 };
 #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */

thirdparty/bullet/BulletSoftBody/btDeformableLinearElasticityForce.h

@@ -18,323 +18,445 @@
 
 #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 damping = 0.05;
-        m_mu_damp = damping * m_mu;
-        m_lambda_damp = damping * m_lambda;
-    }
-    
-    btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda)
-    {
-        m_mu_damp = damping * m_mu;
-        m_lambda_damp = damping * m_lambda;
-    }
-    
-    virtual void addScaledForces(btScalar scale, TVStack& force)
-    {
-        addScaledDampingForce(scale, force);
-        addScaledElasticForce(scale, force);
-    }
-    
-    virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
-    {
-        addScaledElasticForce(scale, force);
-    }
-    
-    // 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)
-            return;
-        int numNodes = getNumNodes();
-        btAssert(numNodes <= force.size());
-        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];
-                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];
-                size_t id0 = node0->index;
-                size_t id1 = node1->index;
-                size_t id2 = node2->index;
-                size_t id3 = node3->index;
-                btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
-                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 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
-                
-                // damping force differential
-                btScalar scale1 = scale * tetra.m_element_measure;
-                force[id0] -= scale1 * df_on_node0;
-                force[id1] -= scale1 * df_on_node123.getColumn(0);
-                force[id2] -= scale1 * df_on_node123.getColumn(1);
-                force[id3] -= scale1 * df_on_node123.getColumn(2);
-            }
-        }
-    }
-    
-    virtual double totalElasticEnergy(btScalar dt)
-    {
-        double energy = 0;
-        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_tetraScratches.size(); ++j)
-            {
-                btSoftBody::Tetra& tetra = psb->m_tetras[j];
-                btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
-                energy += tetra.m_element_measure * elasticEnergyDensity(s);
-            }
-        }
-        return energy;
-    }
-    
-    // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
-    virtual double totalDampingEnergy(btScalar dt)
-    {
-        double energy = 0;
-        int sz = 0;
-        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_nodes.size(); ++j)
-            {
-                sz = btMax(sz, psb->m_nodes[j].index);
-            }
-        }
-        TVStack dampingForce;
-        dampingForce.resize(sz+1);
-        for (int i = 0; i < dampingForce.size(); ++i)
-            dampingForce[i].setZero();
-        addScaledDampingForce(0.5, dampingForce);
-        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];
-                energy -= dampingForce[node.index].dot(node.m_v) / dt;
-            }
-        }
-        return energy;
-    }
-    
-    double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
-    {
-        double density = 0;
-        btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
-        btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
-        density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2());
-        density += m_lambda * trace * trace * 0.5;
-        return density;
-    }
-    
-    virtual void addScaledElasticForce(btScalar scale, TVStack& force)
-    {
-        int numNodes = getNumNodes();
-        btAssert(numNodes <= force.size());
-        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;
-            }
-            btScalar max_p = psb->m_cfg.m_maxStress;
-            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);
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btScalar m_mu, m_lambda;
+	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)
+	{
+		updateYoungsModulusAndPoissonRatio();
+	}
+
+	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)
+	{
+		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)
+	{
+		addScaledDampingForce(scale, force);
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+	{
+		addScaledElasticForce(scale, force);
+	}
+
+	// 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_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);
+		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)
+			{
+				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];
+				btSoftBody::Node* node2 = tetra.m_n[2];
+				btSoftBody::Node* node3 = tetra.m_n[3];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				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()) * 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;
+				force[id1] -= scale1 * df_on_node123.getColumn(0);
+				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;
+				}
+			}
+		}
+	}
+
+	virtual double totalElasticEnergy(btScalar dt)
+	{
+		double energy = 0;
+		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_tetraScratches.size(); ++j)
+			{
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
+				energy += tetra.m_element_measure * elasticEnergyDensity(s);
+			}
+		}
+		return energy;
+	}
+
+	// The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
+	virtual double totalDampingEnergy(btScalar dt)
+	{
+		double energy = 0;
+		int sz = 0;
+		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_nodes.size(); ++j)
+			{
+				sz = btMax(sz, psb->m_nodes[j].index);
+			}
+		}
+		TVStack dampingForce;
+		dampingForce.resize(sz + 1);
+		for (int i = 0; i < dampingForce.size(); ++i)
+			dampingForce[i].setZero();
+		addScaledDampingForce(0.5, dampingForce);
+		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];
+				energy -= dampingForce[node.index].dot(node.m_v) / dt;
+			}
+		}
+		return energy;
+	}
+
+	double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
+	{
+		double density = 0;
+		btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
+		btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
+		density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2());
+		density += m_lambda * trace * trace * 0.5;
+		return density;
+	}
+
+	virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		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;
+			}
+			btScalar max_p = psb->m_cfg.m_maxStress;
+			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);
 #if USE_SVD
-                if (max_p > 0)
-                {
-                    // since we want to clamp the principal stress to max_p, we only need to
-                    // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
-                    btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
-                    if (trPTP > max_p * max_p)
-                    {
-                        btMatrix3x3 U, V;
-                        btVector3 sigma;
-                        singularValueDecomposition(P, U, sigma, V);
-                        sigma[0] = btMin(sigma[0], max_p);
-                        sigma[1] = btMin(sigma[1], max_p);
-                        sigma[2] = btMin(sigma[2], max_p);
-                        sigma[0] = btMax(sigma[0], -max_p);
-                        sigma[1] = btMax(sigma[1], -max_p);
-                        sigma[2] = btMax(sigma[2], -max_p);
-                        btMatrix3x3 Sigma;
-                        Sigma.setIdentity();
-                        Sigma[0][0] = sigma[0];
-                        Sigma[1][1] = sigma[1];
-                        Sigma[2][2] = sigma[2];
-                        P = U * Sigma * V.transpose();
-                    }
-                }
+				if (max_p > 0)
+				{
+					// since we want to clamp the principal stress to max_p, we only need to
+					// calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
+					btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
+					if (trPTP > max_p * max_p)
+					{
+						btMatrix3x3 U, V;
+						btVector3 sigma;
+						singularValueDecomposition(P, U, sigma, V);
+						sigma[0] = btMin(sigma[0], max_p);
+						sigma[1] = btMin(sigma[1], max_p);
+						sigma[2] = btMin(sigma[2], max_p);
+						sigma[0] = btMax(sigma[0], -max_p);
+						sigma[1] = btMax(sigma[1], -max_p);
+						sigma[2] = btMax(sigma[2], -max_p);
+						btMatrix3x3 Sigma;
+						Sigma.setIdentity();
+						Sigma[0][0] = sigma[0];
+						Sigma[1][1] = sigma[1];
+						Sigma[2][2] = sigma[2];
+						P = U * Sigma * V.transpose();
+					}
+				}
 #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();
-                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];
-                size_t id0 = node0->index;
-                size_t id1 = node1->index;
-                size_t id2 = node2->index;
-                size_t id3 = node3->index;
-                
-                // elastic force
-                btScalar scale1 = scale * tetra.m_element_measure;
-                force[id0] -= scale1 * force_on_node0;
-                force[id1] -= scale1 * force_on_node123.getColumn(0);
-                force[id2] -= scale1 * force_on_node123.getColumn(1);
-                force[id3] -= scale1 * force_on_node123.getColumn(2);
-            }
-        }
-    }
-    
-    // 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)
-            return;
-        int numNodes = getNumNodes();
-        btAssert(numNodes <= df.size());
-        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];
-                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];
-                size_t id0 = node0->index;
-                size_t id1 = node1->index;
-                size_t id2 = node2->index;
-                size_t id3 = node3->index;
-                btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
-                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 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
-                btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
-                
-                // damping force differential
-                btScalar scale1 = scale * tetra.m_element_measure;
-                df[id0] -= scale1 * df_on_node0;
-                df[id1] -= scale1 * df_on_node123.getColumn(0);
-                df[id2] -= scale1 * df_on_node123.getColumn(1);
-                df[id3] -= scale1 * df_on_node123.getColumn(2);
-            }
-        }
-    }
-    
-    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
-    {
-        int numNodes = getNumNodes();
-        btAssert(numNodes <= df.size());
-        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];
-                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];
-                size_t id0 = node0->index;
-                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 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();
-                btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
-                
-                // elastic force differential
-                btScalar scale1 = scale * tetra.m_element_measure;
-                df[id0] -= scale1 * df_on_node0;
-                df[id1] -= scale1 * df_on_node123.getColumn(0);
-                df[id2] -= scale1 * df_on_node123.getColumn(1);
-                df[id3] -= scale1 * df_on_node123.getColumn(2);
-            }
-        }
-    }
-    
-    void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
-    {
-        btMatrix3x3 epsilon = (s.m_F + s.m_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;
-    }
-    
-    // Let P be the first piola stress.
-    // This function calculates the dP = dP/dF * dF
-    void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF,  btMatrix3x3& dP)
-    {
-        btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
-        dP = (dF + dF.transpose()) * m_mu +  btMatrix3x3::getIdentity()  * m_lambda * trace;
-    }
-    
-    // Let Q be the damping stress.
-    // This function calculates the dP = dQ/dF * dF
-    void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF,  btMatrix3x3& dP)
-    {
-        btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
-        dP = (dF + dF.transpose()) * m_mu_damp +  btMatrix3x3::getIdentity()  * m_lambda_damp * trace;
-    }
-    
-    virtual btDeformableLagrangianForceType getForceType()
-    {
-        return BT_LINEAR_ELASTICITY_FORCE;
-    }
-    
+				//                btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+				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];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
+
+				// elastic force
+				btScalar scale1 = scale * tetra.m_element_measure;
+				force[id0] -= scale1 * force_on_node0;
+				force[id1] -= scale1 * force_on_node123.getColumn(0);
+				force[id2] -= scale1 * force_on_node123.getColumn(1);
+				force[id3] -= scale1 * force_on_node123.getColumn(2);
+			}
+		}
+	}
+
+	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_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);
+		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)
+			{
+				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];
+				btSoftBody::Node* node2 = tetra.m_n[2];
+				btSoftBody::Node* node3 = tetra.m_n[3];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				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()) * 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;
+				df[id0] -= scale1 * df_on_node0;
+				df[id1] -= scale1 * df_on_node123.getColumn(0);
+				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;
+				}
+			}
+		}
+	}
+
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= df.size());
+		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];
+				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];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
+				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 = 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
+				btScalar scale1 = scale * tetra.m_element_measure;
+				df[id0] -= scale1 * df_on_node0;
+				df[id1] -= scale1 * df_on_node123.getColumn(0);
+				df[id2] -= scale1 * df_on_node123.getColumn(1);
+				df[id3] -= scale1 * df_on_node123.getColumn(2);
+			}
+		}
+	}
+
+	void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
+	{
+		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;
+	}
+
+	// Let P be the first piola stress.
+	// This function calculates the dP = dP/dF * dF
+	void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
+	{
+		btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
+		dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
+	}
+
+	// Let Q be the damping stress.
+	// 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()) * 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

@@ -20,282 +20,282 @@
 
 class btDeformableMassSpringForce : public btDeformableLagrangianForce
 {
-    // If true, the damping force will be in the direction of the spring
-    // If false, the damping force will be in the direction of the velocity
-    bool m_momentum_conserving;
-    btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness;
+	// If true, the damping force will be in the direction of the spring
+	// If false, the damping force will be in the direction of the velocity
+	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)
-    {
-    }
-    btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k)
-    {
-        if (m_bendingStiffness < btScalar(0))
-        {
-            m_bendingStiffness = m_elasticStiffness;
-        }
-    }
-    
-    virtual void addScaledForces(btScalar scale, TVStack& force)
-    {
-        addScaledDampingForce(scale, force);
-        addScaledElasticForce(scale, force);
-    }
-    
-    virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
-    {
-        addScaledElasticForce(scale, force);
-    }
-    
-    virtual void addScaledDampingForce(btScalar scale, TVStack& force)
-    {
-        int numNodes = getNumNodes();
-        btAssert(numNodes <= force.size());
-        for (int i = 0; i < m_softBodies.size(); ++i)
-        {
-            const btSoftBody* psb = m_softBodies[i];
-            if (!psb->isActive())
-            {
-                continue;
-            }
-            for (int j = 0; j < psb->m_links.size(); ++j)
-            {
-                const btSoftBody::Link& link = psb->m_links[j];
-                btSoftBody::Node* node1 = link.m_n[0];
-                btSoftBody::Node* node2 = link.m_n[1];
-                size_t id1 = node1->index;
-                size_t id2 = node2->index;
-                
-                // damping force
-                btVector3 v_diff = (node2->m_v - node1->m_v);
-                btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
-                if (m_momentum_conserving)
-                {
-                    if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
-                    {
-                        btVector3 dir = (node2->m_x - node1->m_x).normalized();
-                        scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
-                    }
-                }
-                force[id1] += scaled_force;
-                force[id2] -= scaled_force;
-            }
-        }
-    }
-    
-    virtual void addScaledElasticForce(btScalar scale, TVStack& force)
-    {
-        int numNodes = getNumNodes();
-        btAssert(numNodes <= force.size());
-        for (int i = 0; i < m_softBodies.size(); ++i)
-        {
-            const btSoftBody* psb = m_softBodies[i];
-            if (!psb->isActive())
-            {
-                continue;
-            }
-            for (int j = 0; j < psb->m_links.size(); ++j)
-            {
-                const btSoftBody::Link& link = psb->m_links[j];
-                btSoftBody::Node* node1 = link.m_n[0];
-                btSoftBody::Node* node2 = link.m_n[1];
-                btScalar r = link.m_rl;
-                size_t id1 = node1->index;
-                size_t id2 = node2->index;
-                
-                // elastic force
-                btVector3 dir = (node2->m_q - node1->m_q);
-                btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
-                btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
-                btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r);
-                force[id1] += scaled_force;
-                force[id2] -= scaled_force;
-            }
-        }
-    }
-    
-    virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
-    {
-        // implicit damping force differential
-        for (int i = 0; i < m_softBodies.size(); ++i)
-        {
-            btSoftBody* psb = m_softBodies[i];
-            if (!psb->isActive())
-            {
-                continue;
-            }
-            btScalar scaled_k_damp = m_dampingStiffness * scale;
-            for (int j = 0; j < psb->m_links.size(); ++j)
-            {
-                const btSoftBody::Link& link = psb->m_links[j];
-                btSoftBody::Node* node1 = link.m_n[0];
-                btSoftBody::Node* node2 = link.m_n[1];
-                size_t id1 = node1->index;
-                size_t id2 = node2->index;
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05)
+	{
+	}
+	btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k)
+	{
+		if (m_bendingStiffness < btScalar(0))
+		{
+			m_bendingStiffness = m_elasticStiffness;
+		}
+	}
+
+	virtual void addScaledForces(btScalar scale, TVStack& force)
+	{
+		addScaledDampingForce(scale, force);
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+	{
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			const btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_links.size(); ++j)
+			{
+				const btSoftBody::Link& link = psb->m_links[j];
+				btSoftBody::Node* node1 = link.m_n[0];
+				btSoftBody::Node* node2 = link.m_n[1];
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+
+				// damping force
+				btVector3 v_diff = (node2->m_v - node1->m_v);
+				btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
+				if (m_momentum_conserving)
+				{
+					if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
+					{
+						btVector3 dir = (node2->m_x - node1->m_x).normalized();
+						scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
+					}
+				}
+				force[id1] += scaled_force;
+				force[id2] -= scaled_force;
+			}
+		}
+	}
+
+	virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			const btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_links.size(); ++j)
+			{
+				const btSoftBody::Link& link = psb->m_links[j];
+				btSoftBody::Node* node1 = link.m_n[0];
+				btSoftBody::Node* node2 = link.m_n[1];
+				btScalar r = link.m_rl;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+
+				// elastic force
+				btVector3 dir = (node2->m_q - node1->m_q);
+				btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
+				btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
+				btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r);
+				force[id1] += scaled_force;
+				force[id2] -= scaled_force;
+			}
+		}
+	}
+
+	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+	{
+		// implicit damping force differential
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			btScalar scaled_k_damp = m_dampingStiffness * scale;
+			for (int j = 0; j < psb->m_links.size(); ++j)
+			{
+				const btSoftBody::Link& link = psb->m_links[j];
+				btSoftBody::Node* node1 = link.m_n[0];
+				btSoftBody::Node* node2 = link.m_n[1];
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+
+				btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
+				if (m_momentum_conserving)
+				{
+					if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
+					{
+						btVector3 dir = (node2->m_x - node1->m_x).normalized();
+						local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
+					}
+				}
+				df[id1] += local_scaled_df;
+				df[id2] -= local_scaled_df;
+			}
+		}
+	}
+
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA)
+	{
+		// implicit damping force differential
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			btScalar scaled_k_damp = m_dampingStiffness * scale;
+			for (int j = 0; j < psb->m_links.size(); ++j)
+			{
+				const btSoftBody::Link& link = psb->m_links[j];
+				btSoftBody::Node* node1 = link.m_n[0];
+				btSoftBody::Node* node2 = link.m_n[1];
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				if (m_momentum_conserving)
+				{
+					if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
+					{
+						btVector3 dir = (node2->m_x - node1->m_x).normalized();
+						for (int d = 0; d < 3; ++d)
+						{
+							if (node1->m_im > 0)
+								diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d];
+							if (node2->m_im > 0)
+								diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d];
+						}
+					}
+				}
+				else
+				{
+					for (int d = 0; d < 3; ++d)
+					{
+						if (node1->m_im > 0)
+							diagA[id1][d] -= scaled_k_damp;
+						if (node2->m_im > 0)
+							diagA[id2][d] -= scaled_k_damp;
+					}
+				}
+			}
+		}
+	}
+
+	virtual double totalElasticEnergy(btScalar dt)
+	{
+		double energy = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			const btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_links.size(); ++j)
+			{
+				const btSoftBody::Link& link = psb->m_links[j];
+				btSoftBody::Node* node1 = link.m_n[0];
+				btSoftBody::Node* node2 = link.m_n[1];
+				btScalar r = link.m_rl;
+
+				// elastic force
+				btVector3 dir = (node2->m_q - node1->m_q);
+				energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() - r);
+			}
+		}
+		return energy;
+	}
+
+	virtual double totalDampingEnergy(btScalar dt)
+	{
+		double energy = 0;
+		int sz = 0;
+		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_nodes.size(); ++j)
+			{
+				sz = btMax(sz, psb->m_nodes[j].index);
+			}
+		}
+		TVStack dampingForce;
+		dampingForce.resize(sz + 1);
+		for (int i = 0; i < dampingForce.size(); ++i)
+			dampingForce[i].setZero();
+		addScaledDampingForce(0.5, dampingForce);
+		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];
+				energy -= dampingForce[node.index].dot(node.m_v) / dt;
+			}
+		}
+		return energy;
+	}
+
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+	{
+		// implicit damping force differential
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			const btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_links.size(); ++j)
+			{
+				const btSoftBody::Link& link = psb->m_links[j];
+				btSoftBody::Node* node1 = link.m_n[0];
+				btSoftBody::Node* node2 = link.m_n[1];
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				btScalar r = link.m_rl;
 
-                btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
-                if (m_momentum_conserving)
-                {
-                    if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
-                    {
-                        btVector3 dir = (node2->m_x - node1->m_x).normalized();
-                        local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
-                    }
-                }
-                df[id1] += local_scaled_df;
-                df[id2] -= local_scaled_df;
-            }
-        }
-    }
-    
-    virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA)
-    {
-        // implicit damping force differential
-        for (int i = 0; i < m_softBodies.size(); ++i)
-        {
-            btSoftBody* psb = m_softBodies[i];
-            if (!psb->isActive())
-            {
-                continue;
-            }
-            btScalar scaled_k_damp = m_dampingStiffness * scale;
-            for (int j = 0; j < psb->m_links.size(); ++j)
-            {
-                const btSoftBody::Link& link = psb->m_links[j];
-                btSoftBody::Node* node1 = link.m_n[0];
-                btSoftBody::Node* node2 = link.m_n[1];
-                size_t id1 = node1->index;
-                size_t id2 = node2->index;
-                if (m_momentum_conserving)
-                {
-                    if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
-                    {
-                        btVector3 dir = (node2->m_x - node1->m_x).normalized();
-                        for (int d = 0; d < 3; ++d)
-                        {
-                            if (node1->m_im > 0)
-                                diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d];
-                            if (node2->m_im > 0)
-                                diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d];
-                        }
-                    }
-                }
-                else
-                {
-                    for (int d = 0; d < 3; ++d)
-                    {
-                        if (node1->m_im > 0)
-                            diagA[id1][d] -= scaled_k_damp;
-                        if (node2->m_im > 0)
-                            diagA[id2][d] -= scaled_k_damp;
-                    }
-                }
-            }
-        }
-    }
-    
-    virtual double totalElasticEnergy(btScalar dt)
-    {
-        double energy = 0;
-        for (int i = 0; i < m_softBodies.size(); ++i)
-        {
-            const btSoftBody* psb = m_softBodies[i];
-            if (!psb->isActive())
-            {
-                continue;
-            }
-            for (int j = 0; j < psb->m_links.size(); ++j)
-            {
-                const btSoftBody::Link& link = psb->m_links[j];
-                btSoftBody::Node* node1 = link.m_n[0];
-                btSoftBody::Node* node2 = link.m_n[1];
-                btScalar r = link.m_rl;
+				btVector3 dir = (node1->m_q - node2->m_q);
+				btScalar dir_norm = dir.norm();
+				btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
+				btVector3 dx_diff = dx[id1] - dx[id2];
+				btVector3 scaled_df = btVector3(0, 0, 0);
+				btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
+				if (dir_norm > SIMD_EPSILON)
+				{
+					scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized;
+					scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized;
+					scaled_df -= scaled_k * ((dir_norm - r) / dir_norm) * dx_diff;
+				}
 
-                // elastic force
-                btVector3 dir = (node2->m_q - node1->m_q);
-                energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r);
-            }
-        }
-        return energy;
-    }
-    
-    virtual double totalDampingEnergy(btScalar dt)
-    {
-        double energy = 0;
-        int sz = 0;
-        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_nodes.size(); ++j)
-            {
-                sz = btMax(sz, psb->m_nodes[j].index);
-            }
-        }
-        TVStack dampingForce;
-        dampingForce.resize(sz+1);
-        for (int i = 0; i < dampingForce.size(); ++i)
-            dampingForce[i].setZero();
-        addScaledDampingForce(0.5, dampingForce);
-        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];
-                energy -= dampingForce[node.index].dot(node.m_v) / dt;
-            }
-        }
-        return energy;
-    }
-    
-    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
-    {
-        // implicit damping force differential
-        for (int i = 0; i < m_softBodies.size(); ++i)
-        {
-            const btSoftBody* psb = m_softBodies[i];
-            if (!psb->isActive())
-            {
-                continue;
-            }
-            for (int j = 0; j < psb->m_links.size(); ++j)
-            {
-                const btSoftBody::Link& link = psb->m_links[j];
-                btSoftBody::Node* node1 = link.m_n[0];
-                btSoftBody::Node* node2 = link.m_n[1];
-                size_t id1 = node1->index;
-                size_t id2 = node2->index;
-                btScalar r = link.m_rl;
+				df[id1] += scaled_df;
+				df[id2] -= scaled_df;
+			}
+		}
+	}
 
-                btVector3 dir = (node1->m_q - node2->m_q);
-                btScalar dir_norm = dir.norm();
-                btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
-                btVector3 dx_diff = dx[id1] - dx[id2];
-                btVector3 scaled_df = btVector3(0,0,0);
-                btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
-                if (dir_norm > SIMD_EPSILON)
-                {
-                    scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized;
-                    scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm-r)/dir_norm) * dir_normalized;
-                    scaled_df -= scaled_k * ((dir_norm-r)/dir_norm) * dx_diff;
-                }
-                
-                df[id1] += scaled_df;
-                df[id2] -= scaled_df;
-            }
-        }
-    }
-    
-    virtual btDeformableLagrangianForceType getForceType()
-    {
-        return BT_MASSSPRING_FORCE;
-    }
-    
+	virtual btDeformableLagrangianForceType getForceType()
+	{
+		return BT_MASSSPRING_FORCE;
+	}
 };
 
 #endif /* btMassSpring_h */

thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h

@@ -20,126 +20,143 @@
 
 class btDeformableMousePickingForce : public btDeformableLagrangianForce
 {
-    // If true, the damping force will be in the direction of the spring
-    // If false, the damping force will be in the direction of the velocity
-    btScalar m_elasticStiffness, m_dampingStiffness;
-    const btSoftBody::Face& m_face;
-    btVector3 m_mouse_pos;
-    btScalar m_maxForce;
+	// If true, the damping force will be in the direction of the spring
+	// If false, the damping force will be in the direction of the velocity
+	btScalar m_elasticStiffness, m_dampingStiffness;
+	const btSoftBody::Face& m_face;
+	btVector3 m_mouse_pos;
+	btScalar m_maxForce;
+
 public:
-    typedef btAlignedObjectArray<btVector3> TVStack;
-    btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos),  m_maxForce(maxForce)
-    {
-    }
-    
-    virtual void addScaledForces(btScalar scale, TVStack& force)
-    {
-        addScaledDampingForce(scale, force);
-        addScaledElasticForce(scale, force);
-    }
-    
-    virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
-    {
-        addScaledElasticForce(scale, force);
-    }
-    
-    virtual void addScaledDampingForce(btScalar scale, TVStack& force)
-    {
-        for (int i = 0; i < 3; ++i)
-        {
-            btVector3 v_diff = m_face.m_n[i]->m_v;
-            btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
-            if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
-            {
-                btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
-                scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
-            }
-            force[m_face.m_n[i]->index] -= scaled_force;
-        }
-    }
-    
-    virtual void addScaledElasticForce(btScalar scale, TVStack& force)
-    {
-        btScalar scaled_stiffness = scale * m_elasticStiffness;
-        for (int i = 0; i < 3; ++i)
-        {
-            btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
-            btVector3 scaled_force = scaled_stiffness * dir;
-            if (scaled_force.safeNorm() > m_maxForce)
-            {
-                scaled_force.safeNormalize();
-                scaled_force *= m_maxForce;
-            }
-            force[m_face.m_n[i]->index] -= scaled_force;
-        }
-    }
-    
-    virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
-    {
-        btScalar scaled_k_damp = m_dampingStiffness * scale;
-        for (int i = 0; i < 3; ++i)
-        {
-            btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index];
-            if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
-            {
-                btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
-                local_scaled_df= scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir;
-            }
-            df[m_face.m_n[i]->index] -= local_scaled_df;
-        }
-    }
-    
-    virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){}
-    
-    virtual double totalElasticEnergy(btScalar dt)
-    {
-        double energy = 0;
-        for (int i = 0; i < 3; ++i)
-        {
-            btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
-            btVector3 scaled_force = m_elasticStiffness * dir;
-            if (scaled_force.safeNorm() > m_maxForce)
-            {
-                scaled_force.safeNormalize();
-                scaled_force *= m_maxForce;
-            }
-            energy += 0.5 * scaled_force.dot(dir);
-        }
-        return energy;
-    }
-    
-    virtual double totalDampingEnergy(btScalar dt)
-    {
-        double energy = 0;
-        for (int i = 0; i < 3; ++i)
-        {
-            btVector3 v_diff = m_face.m_n[i]->m_v;
-            btVector3 scaled_force = m_dampingStiffness * v_diff;
-            if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
-            {
-                btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
-                scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir;
-            }
-            energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt;
-        }
-        return energy;
-    }
-    
-    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
-    {
-        //TODO
-    }
-    
-    void setMousePos(const btVector3& p)
-    {
-        m_mouse_pos = p;
-    }
-    
-    virtual btDeformableLagrangianForceType getForceType()
-    {
-        return BT_MOUSE_PICKING_FORCE;
-    }
-    
+	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)
+	{
+	}
+
+	virtual void addScaledForces(btScalar scale, TVStack& force)
+	{
+		addScaledDampingForce(scale, force);
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+	{
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+	{
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 v_diff = m_face.m_n[i]->m_v;
+			btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
+			if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
+			{
+				btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
+				scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
+			}
+			force[m_face.m_n[i]->index] -= scaled_force;
+		}
+	}
+
+	virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+	{
+		btScalar scaled_stiffness = scale * m_elasticStiffness;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
+			btVector3 scaled_force = scaled_stiffness * dir;
+			if (scaled_force.safeNorm() > m_maxForce)
+			{
+				scaled_force.safeNormalize();
+				scaled_force *= m_maxForce;
+			}
+			force[m_face.m_n[i]->index] -= scaled_force;
+		}
+	}
+
+	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+	{
+		btScalar scaled_k_damp = m_dampingStiffness * scale;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index];
+			if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
+			{
+				btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
+				local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir;
+			}
+			df[m_face.m_n[i]->index] -= local_scaled_df;
+		}
+	}
+
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
+
+	virtual double totalElasticEnergy(btScalar dt)
+	{
+		double energy = 0;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
+			btVector3 scaled_force = m_elasticStiffness * dir;
+			if (scaled_force.safeNorm() > m_maxForce)
+			{
+				scaled_force.safeNormalize();
+				scaled_force *= m_maxForce;
+			}
+			energy += 0.5 * scaled_force.dot(dir);
+		}
+		return energy;
+	}
+
+	virtual double totalDampingEnergy(btScalar dt)
+	{
+		double energy = 0;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 v_diff = m_face.m_n[i]->m_v;
+			btVector3 scaled_force = m_dampingStiffness * v_diff;
+			if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
+			{
+				btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
+				scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir;
+			}
+			energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt;
+		}
+		return energy;
+	}
+
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+	{
+		btScalar scaled_stiffness = scale * m_elasticStiffness;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
+			btScalar dir_norm = dir.norm();
+			btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
+			int id = m_face.m_n[i]->index;
+			btVector3 dx_diff = dx[id];
+			btScalar r = 0;  // rest length is 0 for picking spring
+			btVector3 scaled_df = btVector3(0, 0, 0);
+			if (dir_norm > SIMD_EPSILON)
+			{
+				scaled_df -= scaled_stiffness * dir_normalized.dot(dx_diff) * dir_normalized;
+				scaled_df += scaled_stiffness * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized;
+				scaled_df -= scaled_stiffness * ((dir_norm - r) / dir_norm) * dx_diff;
+			}
+			df[id] += scaled_df;
+		}
+	}
+
+	void setMousePos(const btVector3& p)
+	{
+		m_mouse_pos = p;
+	}
+
+	virtual btDeformableLagrangianForceType getForceType()
+	{
+		return BT_MOUSE_PICKING_FORCE;
+	}
 };
 
 #endif /* btMassSpring_h */

thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -13,131 +13,132 @@
  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
-btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
-    {
-        ///this is a special step to resolve penetrations (just for contacts)
-        solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+	{
+		///this is a special step to resolve penetrations (just for contacts)
+		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++)
+		{
+			// rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body
 
-        int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
-        for (int iteration = 0; iteration < maxIterations; iteration++)
-        {
-            // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body
-            
-            // solve rigid/rigid in solver body
-            m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
-            // solver body velocity -> rigid body velocity
-            solverBodyWriteBack(infoGlobal);
-            btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies, infoGlobal);
-            // update rigid body velocity in rigid/deformable contact
-            m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
-            // solver body velocity <- rigid body velocity
-            writeToSolverBody(bodies, numBodies, infoGlobal);
-            
-            if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
-            {
+			// solve rigid/rigid in solver body
+			m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+			// solver body velocity -> rigid body velocity
+			solverBodyWriteBack(infoGlobal);
+			btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies, numDeformableBodies, infoGlobal);
+			// update rigid body velocity in rigid/deformable contact
+			m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
+			// solver body velocity <- rigid body velocity
+			writeToSolverBody(bodies, numBodies, infoGlobal);
+
+			if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
+			{
 #ifdef VERBOSE_RESIDUAL_PRINTF
-                printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
+				if (iteration >= (maxIterations - 1))
+					printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
 #endif
-                m_analyticsData.m_numSolverCalls++;
-                m_analyticsData.m_numIterationsUsed = iteration+1;
-                m_analyticsData.m_islandId = -2;
-                if (numBodies>0)
-                    m_analyticsData.m_islandId = bodies[0]->getCompanionId();
-                m_analyticsData.m_numBodies = numBodies;
-                m_analyticsData.m_numContactManifolds = numManifolds;
-                m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
-                break;
-            }
-        }
-    }
-    return 0.f;
+				m_analyticsData.m_numSolverCalls++;
+				m_analyticsData.m_numIterationsUsed = iteration + 1;
+				m_analyticsData.m_islandId = -2;
+				if (numBodies > 0)
+					m_analyticsData.m_islandId = bodies[0]->getCompanionId();
+				m_analyticsData.m_numBodies = numBodies;
+				m_analyticsData.m_numContactManifolds = numManifolds;
+				m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
+				break;
+			}
+		}
+	}
+	return 0.f;
 }
 
-void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
 {
-    m_tmpMultiBodyConstraints = multiBodyConstraints;
-    m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
-    
-    // inherited from MultiBodyConstraintSolver
-    solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
-    
-    // overriden
-    solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
-    
-    // inherited from MultiBodyConstraintSolver
-    solveGroupCacheFriendlyFinish(bodies, numBodies, info);
-    
-    m_tmpMultiBodyConstraints = 0;
-    m_tmpNumMultiBodyConstraints = 0;
+	m_tmpMultiBodyConstraints = multiBodyConstraints;
+	m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
+
+	// inherited from MultiBodyConstraintSolver
+	solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+
+	// overriden
+	solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+
+	// inherited from MultiBodyConstraintSolver
+	solveGroupCacheFriendlyFinish(bodies, numBodies, info);
+
+	m_tmpMultiBodyConstraints = 0;
+	m_tmpNumMultiBodyConstraints = 0;
 }
 
 void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
 {
-    for (int i = 0; i < numBodies; i++)
-    {
-        int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
+	for (int i = 0; i < numBodies; i++)
+	{
+		int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
 
-        btRigidBody* body = btRigidBody::upcast(bodies[i]);
-        if (body && body->getInvMass())
-        {
-            btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
-            solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
-            solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
-        }
-    }
+		btRigidBody* body = btRigidBody::upcast(bodies[i]);
+		if (body && body->getInvMass())
+		{
+			btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
+			solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
+			solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
+		}
+	}
 }
 
 void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
 {
-    for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
-    {
-        btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
-        if (body)
-        {
-            m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
-            m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
-        }
-    }
+	for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
+	{
+		btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
+		if (body)
+		{
+			m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
+			m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity + m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
+		}
+	}
 }
 
-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;
-                {
-                    int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-                    int j;
-                    for (j = 0; j < numPoolConstraints; j++)
-                    {
-                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-                        
-                        btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
-                        leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
-                    }
-                    // solve the position correction between deformable and rigid/multibody
-//                    btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
-//                    leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
-                }
-                if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
-                {
+	BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
+	int iteration;
+	if (infoGlobal.m_splitImpulse)
+	{
+		{
+			for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
+			{
+				btScalar leastSquaresResidual = 0.f;
+				{
+					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+					int j;
+					for (j = 0; j < numPoolConstraints; j++)
+					{
+						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+
+						btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+						leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+					}
+					// solve the position correction between deformable and rigid/multibody
+					//                    btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
+					btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal);
+					leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+				}
+				if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
+				{
 #ifdef VERBOSE_RESIDUAL_PRINTF
-                    printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
+					if (iteration >= (infoGlobal.m_numIterations - 1))
+						printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
 #endif
-                    break;
-                }
-            }
-        }
-    }
+					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
 
@@ -32,30 +31,31 @@ class btDeformableBodySolver;
 ATTRIBUTE_ALIGNED16(class)
 btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver
 {
-    btDeformableBodySolver* m_deformableSolver;
-    
+	btDeformableBodySolver* m_deformableSolver;
+
 protected:
-    // override the iterations method to include deformable/multibody contact
-//    virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-    
-    // write the velocity of the the solver body to the underlying rigid body
-    void solverBodyWriteBack(const btContactSolverInfo& infoGlobal);
-
-    // 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 btScalar solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+	// override the iterations method to include deformable/multibody contact
+	//    virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+
+	// write the velocity of the the solver body to the underlying rigid body
+	void solverBodyWriteBack(const btContactSolverInfo& infoGlobal);
+
+	// 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, 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();
-    
-    void setDeformableSolver(btDeformableBodySolver* deformableSolver)
-    {
-        m_deformableSolver = deformableSolver;
-    }
-    
-    virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	void setDeformableSolver(btDeformableBodySolver * deformableSolver)
+	{
+		m_deformableSolver = deformableSolver;
+	}
+
+	virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
 };
 
 #endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */

thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -40,8 +40,9 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed
 #include "LinearMath/btQuickprof.h"
 #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)
+	: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
+	  m_deformableBodySolver(deformableBodySolver),
+	  m_solverCallback(0)
 {
 	m_drawFlags = fDrawFlags::Std;
 	m_drawNodeTree = true;
@@ -52,7 +53,7 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
 	m_sbi.m_sparsesdf.Initialize();
 	m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005);
 	m_sbi.m_sparsesdf.Reset();
-	
+
 	m_sbi.air_density = (btScalar)1.2;
 	m_sbi.water_density = 0;
 	m_sbi.water_offset = 0;
@@ -61,57 +62,57 @@ 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);
 }
 
 btDeformableMultiBodyDynamicsWorld::~btDeformableMultiBodyDynamicsWorld()
 {
-    delete m_solverDeformableBodyIslandCallback;
+	delete m_solverDeformableBodyIslandCallback;
 }
 
 void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
-    BT_PROFILE("internalSingleStepSimulation");
-    if (0 != m_internalPreTickCallback)
-    {
-        (*m_internalPreTickCallback)(this, timeStep);
-    }
-    reinitialize(timeStep);
-    
-    // add gravity to velocity of rigid and multi bodys
-    applyRigidBodyGravity(timeStep);
-    
-    ///apply gravity and explicit force to velocity, predict motion
-    predictUnconstraintMotion(timeStep);
-    
-    ///perform collision detection that involves rigid/multi bodies
-    btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
-    
-    btMultiBodyDynamicsWorld::calculateSimulationIslands();
-    
-    beforeSolverCallbacks(timeStep);
-    
-    ///solve contact constraints and then deformable bodies momemtum equation
-    solveConstraints(timeStep);
-    
-    afterSolverCallbacks(timeStep);
+	BT_PROFILE("internalSingleStepSimulation");
+	if (0 != m_internalPreTickCallback)
+	{
+		(*m_internalPreTickCallback)(this, timeStep);
+	}
+	reinitialize(timeStep);
+
+	// add gravity to velocity of rigid and multi bodys
+	applyRigidBodyGravity(timeStep);
+
+	///apply gravity and explicit force to velocity, predict motion
+	predictUnconstraintMotion(timeStep);
+
+	///perform collision detection that involves rigid/multi bodies
+	btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
+
+	btMultiBodyDynamicsWorld::calculateSimulationIslands();
+
+	beforeSolverCallbacks(timeStep);
+
+	///solve contact constraints and then deformable bodies momemtum equation
+	solveConstraints(timeStep);
+
+	afterSolverCallbacks(timeStep);
 
 	performDeformableCollisionDetection();
 
-    applyRepulsionForce(timeStep);
+	applyRepulsionForce(timeStep);
+
+	performGeometricCollisions(timeStep);
+
+	integrateTransforms(timeStep);
 
-    performGeometricCollisions(timeStep);
+	///update vehicle simulation
+	btMultiBodyDynamicsWorld::updateActions(timeStep);
 
-    integrateTransforms(timeStep);
-    
-    ///update vehicle simulation
-    btMultiBodyDynamicsWorld::updateActions(timeStep);
-    
-    updateActivationState(timeStep);
-    // End solver-wise simulation step
-    // ///////////////////////////////
+	updateActivationState(timeStep);
+	// End solver-wise simulation step
+	// ///////////////////////////////
 }
 
 void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection()
@@ -120,7 +121,7 @@ void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection()
 	{
 		m_softBodies[i]->m_softSoftCollision = true;
 	}
-	
+
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
 		for (int j = i; j < m_softBodies.size(); ++j)
@@ -128,7 +129,7 @@ void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection()
 			m_softBodies[i]->defaultCollisionHandler(m_softBodies[j]);
 		}
 	}
-	
+
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
 		m_softBodies[i]->m_softSoftCollision = false;
@@ -137,45 +138,45 @@ void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection()
 
 void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
 {
-    for (int i = 0; i < m_softBodies.size(); i++)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        psb->updateDeactivation(timeStep);
-        if (psb->wantsSleeping())
-        {
-            if (psb->getActivationState() == ACTIVE_TAG)
-                psb->setActivationState(WANTS_DEACTIVATION);
-            if (psb->getActivationState() == ISLAND_SLEEPING)
-            {
-                psb->setZeroVelocity();
-            }
-        }
-        else
-        {
-            if (psb->getActivationState() != DISABLE_DEACTIVATION)
-                psb->setActivationState(ACTIVE_TAG);
-        }
-    }
-    btMultiBodyDynamicsWorld::updateActivationState(timeStep);
+	for (int i = 0; i < m_softBodies.size(); i++)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		psb->updateDeactivation(timeStep);
+		if (psb->wantsSleeping())
+		{
+			if (psb->getActivationState() == ACTIVE_TAG)
+				psb->setActivationState(WANTS_DEACTIVATION);
+			if (psb->getActivationState() == ISLAND_SLEEPING)
+			{
+				psb->setZeroVelocity();
+			}
+		}
+		else
+		{
+			if (psb->getActivationState() != DISABLE_DEACTIVATION)
+				psb->setActivationState(ACTIVE_TAG);
+		}
+	}
+	btMultiBodyDynamicsWorld::updateActivationState(timeStep);
 }
 
 void btDeformableMultiBodyDynamicsWorld::applyRepulsionForce(btScalar timeStep)
 {
-    BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce");
-    for (int i = 0; i < m_softBodies.size(); i++)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        if (psb->isActive())
-        {
+	BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce");
+	for (int i = 0; i < m_softBodies.size(); i++)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (psb->isActive())
+		{
 			psb->applyRepulsionForce(timeStep, true);
-        }
-    }
+		}
+	}
 }
 
 void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar timeStep)
 {
 	BT_PROFILE("btDeformableMultiBodyDynamicsWorld::performGeometricCollisions");
-    // refit the BVH tree for CCD
+	// refit the BVH tree for CCD
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
 		btSoftBody* psb = m_softBodies[i];
@@ -214,7 +215,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim
 					f.m_vn = (f.m_n[1]->m_v - f.m_n[0]->m_v).cross(f.m_n[2]->m_v - f.m_n[0]->m_v) * timeStep * timeStep;
 				}
 			}
-        }
+		}
 
 		// apply CCD to register new contact points
 		for (int i = 0; i < m_softBodies.size(); ++i)
@@ -228,7 +229,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim
 					m_softBodies[i]->geometricCollisionHandler(m_softBodies[j]);
 				}
 			}
-        }
+		}
 
 		int penetration_count = 0;
 		for (int i = 0; i < m_softBodies.size(); ++i)
@@ -258,294 +259,292 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim
 
 void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
 {
-    BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision");
-    for (int i = 0; i < m_softBodies.size(); i++)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        if (psb->isActive())
-        {
-            psb->defaultCollisionHandler(psb);
-        }
-    }
+	BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision");
+	for (int i = 0; i < m_softBodies.size(); i++)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (psb->isActive())
+		{
+			psb->defaultCollisionHandler(psb);
+		}
+	}
 }
 
 void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep)
 {
-    // correct the position of rigid bodies with temporary velocity generated from split impulse
-    btContactSolverInfo infoGlobal;
-    btVector3 zero(0,0,0);
-    for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
-    {
-        btRigidBody* rb = m_nonStaticRigidBodies[i];
-        //correct the position/orientation based on push/turn recovery
-        btTransform newTransform;
-        btVector3 pushVelocity = rb->getPushVelocity();
-        btVector3 turnVelocity = rb->getTurnVelocity();
-        if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0)
-        {
-            btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform);
-            rb->setWorldTransform(newTransform);
-            rb->setPushVelocity(zero);
-            rb->setTurnVelocity(zero);
-        }
-    }
+	// correct the position of rigid bodies with temporary velocity generated from split impulse
+	btContactSolverInfo infoGlobal;
+	btVector3 zero(0, 0, 0);
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
+	{
+		btRigidBody* rb = m_nonStaticRigidBodies[i];
+		//correct the position/orientation based on push/turn recovery
+		btTransform newTransform;
+		btVector3 pushVelocity = rb->getPushVelocity();
+		btVector3 turnVelocity = rb->getTurnVelocity();
+		if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0)
+		{
+			btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform);
+			rb->setWorldTransform(newTransform);
+			rb->setPushVelocity(zero);
+			rb->setTurnVelocity(zero);
+		}
+	}
 }
 
 void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
-    BT_PROFILE("integrateTransforms");
-    positionCorrection(timeStep);
-    btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        for (int j = 0; j < psb->m_nodes.size(); ++j)
-        {
-            btSoftBody::Node& node = psb->m_nodes[j];
-            btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
-            btScalar clampDeltaV = maxDisplacement / timeStep;
-            for (int c = 0; c < 3; c++)
-            {
-                if (node.m_v[c] > clampDeltaV)
-                {
-                    node.m_v[c] = clampDeltaV;
-                }
-                if (node.m_v[c] < -clampDeltaV)
-                {
-                    node.m_v[c] = -clampDeltaV;
-                }
-            }
-            node.m_x  =  node.m_x + timeStep * node.m_v;
-            node.m_q = node.m_x;
-            node.m_vn = node.m_v;
-        }
-        // enforce anchor constraints
-        for (int j = 0; j < psb->m_deformableAnchors.size();++j)
-        {
-            btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
-            btSoftBody::Node* n = a.m_node;
-            n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
-            
-            // update multibody anchor info
-            if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
-            {
-                btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
-                if (multibodyLinkCol)
-                {
-                    btVector3 nrm;
-                    const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
-                    const btTransform& wtr = multibodyLinkCol->getWorldTransform();
-                    psb->m_worldInfo->m_sparsesdf.Evaluate(
-                                                      wtr.invXform(n->m_x),
-                                                      shp,
-                                                      nrm,
-                                                      0);
-                    a.m_cti.m_normal = wtr.getBasis() * nrm;
-                    btVector3 normal = a.m_cti.m_normal;
-                    btVector3 t1 = generateUnitOrthogonalVector(normal);
-                    btVector3 t2 = btCross(normal, t1);
-                    btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
-                    findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
-                    findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
-                    findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2);
-            
-                    btScalar* J_n = &jacobianData_normal.m_jacobians[0];
-                    btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
-                    btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
-                    
-                    btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
-                    btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
-                    btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
-                    
-                    btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
-                                    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();
-                    a.m_c0 =  rot.transpose() * local_impulse_matrix * rot;
-                    a.jacobianData_normal = jacobianData_normal;
-                    a.jacobianData_t1 = jacobianData_t1;
-                    a.jacobianData_t2 = jacobianData_t2;
-                    a.t1 = t1;
-                    a.t2 = t2;
-                }
-            }
-        }
-        psb->interpolateRenderMesh();
-    }
+	BT_PROFILE("integrateTransforms");
+	positionCorrection(timeStep);
+	btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			btSoftBody::Node& node = psb->m_nodes[j];
+			btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
+			btScalar clampDeltaV = maxDisplacement / timeStep;
+			for (int c = 0; c < 3; c++)
+			{
+				if (node.m_v[c] > clampDeltaV)
+				{
+					node.m_v[c] = clampDeltaV;
+				}
+				if (node.m_v[c] < -clampDeltaV)
+				{
+					node.m_v[c] = -clampDeltaV;
+				}
+			}
+			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;
+		}
+		// enforce anchor constraints
+		for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
+		{
+			btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
+			btSoftBody::Node* n = a.m_node;
+			n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
+
+			// update multibody anchor info
+			if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+			{
+				btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
+				if (multibodyLinkCol)
+				{
+					btVector3 nrm;
+					const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
+					const btTransform& wtr = multibodyLinkCol->getWorldTransform();
+					psb->m_worldInfo->m_sparsesdf.Evaluate(
+						wtr.invXform(n->m_x),
+						shp,
+						nrm,
+						0);
+					a.m_cti.m_normal = wtr.getBasis() * nrm;
+					btVector3 normal = a.m_cti.m_normal;
+					btVector3 t1 = generateUnitOrthogonalVector(normal);
+					btVector3 t2 = btCross(normal, t1);
+					btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+					findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
+					findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
+					findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2);
+
+					btScalar* J_n = &jacobianData_normal.m_jacobians[0];
+					btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
+					btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
+
+					btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+					btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+					btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+
+					btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
+									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();
+					a.m_c0 = rot.transpose() * local_impulse_matrix * rot;
+					a.jacobianData_normal = jacobianData_normal;
+					a.jacobianData_t1 = jacobianData_t1;
+					a.jacobianData_t2 = jacobianData_t2;
+					a.t1 = t1;
+					a.t2 = t2;
+				}
+			}
+		}
+		psb->interpolateRenderMesh();
+	}
 }
 
 void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 {
-    BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints");
-    // save v_{n+1}^* velocity after explicit forces
-    m_deformableBodySolver->backupVelocity();
-    
-    // set up constraints among multibodies and between multibodies and deformable bodies
-    setupConstraints();
-    
-    // solve contact constraints
-    solveContactConstraints();
-    
-    // set up the directions in which the velocity does not change in the momentum solve
-    if (m_useProjection)
-        m_deformableBodySolver->m_objective->m_projection.setProjection();
-    else
-        m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier();
-
-    // for explicit scheme, m_backupVelocity = v_{n+1}^*
-    // for implicit scheme, m_backupVelocity = v_n
-    // Here, set dv = v_{n+1} - v_n for nodes in contact
-    m_deformableBodySolver->setupDeformableSolve(m_implicit);
-    
-    // At this point, dv should be golden for nodes in contact
-    // proceed to solve deformable momentum equation
-    m_deformableBodySolver->solveDeformableConstraints(timeStep);
+	BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints");
+	// save v_{n+1}^* velocity after explicit forces
+	m_deformableBodySolver->backupVelocity();
+
+	// set up constraints among multibodies and between multibodies and deformable bodies
+	setupConstraints();
+
+	// solve contact constraints
+	solveContactConstraints();
+
+	// set up the directions in which the velocity does not change in the momentum solve
+	if (m_useProjection)
+		m_deformableBodySolver->m_objective->m_projection.setProjection();
+	else
+		m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier();
+
+	// for explicit scheme, m_backupVelocity = v_{n+1}^*
+	// for implicit scheme, m_backupVelocity = v_n
+	// Here, set dv = v_{n+1} - v_n for nodes in contact
+	m_deformableBodySolver->setupDeformableSolve(m_implicit);
+
+	// At this point, dv should be golden for nodes in contact
+	// proceed to solve deformable momentum equation
+	m_deformableBodySolver->solveDeformableConstraints(timeStep);
 }
 
 void btDeformableMultiBodyDynamicsWorld::setupConstraints()
 {
-    // set up constraints between multibody and deformable bodies
-    m_deformableBodySolver->setConstraints(m_solverInfo);
-    
-    // set up constraints among multibodies
-    {
-        sortConstraints();
-        // setup the solver callback
-        btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
-        btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
-        m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
-        
-        // build islands
-        m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
-    }
+	// set up constraints between multibody and deformable bodies
+	m_deformableBodySolver->setConstraints(m_solverInfo);
+
+	// set up constraints among multibodies
+	{
+		sortConstraints();
+		// setup the solver callback
+		btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
+		btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
+		m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
+
+		// build islands
+		m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
+	}
 }
 
 void btDeformableMultiBodyDynamicsWorld::sortConstraints()
 {
-    m_sortedConstraints.resize(m_constraints.size());
-    int i;
-    for (i = 0; i < getNumConstraints(); i++)
-    {
-        m_sortedConstraints[i] = m_constraints[i];
-    }
-    m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
-    
-    m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
-    for (i = 0; i < m_multiBodyConstraints.size(); i++)
-    {
-        m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
-    }
-    m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
+	m_sortedConstraints.resize(m_constraints.size());
+	int i;
+	for (i = 0; i < getNumConstraints(); i++)
+	{
+		m_sortedConstraints[i] = m_constraints[i];
+	}
+	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
+
+	m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
+	for (i = 0; i < m_multiBodyConstraints.size(); i++)
+	{
+		m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
+	}
+	m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
 }
-    
-    
+
 void btDeformableMultiBodyDynamicsWorld::solveContactConstraints()
 {
-    // process constraints on each island
-    m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback);
-    
-    // process deferred
-    m_solverDeformableBodyIslandCallback->processConstraints();
-    m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer);
-    
-    // write joint feedback
-    {
-        for (int i = 0; i < this->m_multiBodies.size(); i++)
-        {
-            btMultiBody* bod = m_multiBodies[i];
-            
-            bool isSleeping = false;
-            
-            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-            {
-                isSleeping = true;
-            }
-            for (int b = 0; b < bod->getNumLinks(); b++)
-            {
-                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
-                    isSleeping = true;
-            }
-            
-            if (!isSleeping)
-            {
-                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
-                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
-                m_scratch_v.resize(bod->getNumLinks() + 1);
-                m_scratch_m.resize(bod->getNumLinks() + 1);
-                
-                if (bod->internalNeedsJointFeedback())
-                {
-                    if (!bod->isUsingRK4Integration())
-                    {
-                        if (bod->internalNeedsJointFeedback())
-                        {
-                            bool isConstraintPass = true;
-                            bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
-                                                                                      getSolverInfo().m_jointFeedbackInWorldSpace,
-                                                                                      getSolverInfo().m_jointFeedbackInJointFrame);
-                        }
-                    }
-                }
-            }
-        }
-    }
-    
-    for (int i = 0; i < this->m_multiBodies.size(); i++)
-    {
-        btMultiBody* bod = m_multiBodies[i];
-        bod->processDeltaVeeMultiDof2();
-    }
+	// process constraints on each island
+	m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback);
+
+	// process deferred
+	m_solverDeformableBodyIslandCallback->processConstraints();
+	m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer);
+
+	// write joint feedback
+	{
+		for (int i = 0; i < this->m_multiBodies.size(); i++)
+		{
+			btMultiBody* bod = m_multiBodies[i];
+
+			bool isSleeping = false;
+
+			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+			{
+				isSleeping = true;
+			}
+			for (int b = 0; b < bod->getNumLinks(); b++)
+			{
+				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+					isSleeping = true;
+			}
+
+			if (!isSleeping)
+			{
+				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+				m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+				m_scratch_v.resize(bod->getNumLinks() + 1);
+				m_scratch_m.resize(bod->getNumLinks() + 1);
+
+				if (bod->internalNeedsJointFeedback())
+				{
+					if (!bod->isUsingRK4Integration())
+					{
+						if (bod->internalNeedsJointFeedback())
+						{
+							bool isConstraintPass = true;
+							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+																					  getSolverInfo().m_jointFeedbackInWorldSpace,
+																					  getSolverInfo().m_jointFeedbackInJointFrame);
+						}
+					}
+				}
+			}
+		}
+	}
+
+	for (int i = 0; i < this->m_multiBodies.size(); i++)
+	{
+		btMultiBody* bod = m_multiBodies[i];
+		bod->processDeltaVeeMultiDof2();
+	}
 }
 
 void btDeformableMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask)
 {
-    m_softBodies.push_back(body);
-    
-    // Set the soft body solver that will deal with this body
-    // to be the world's solver
-    body->setSoftBodySolver(m_deformableBodySolver);
-    
-    btCollisionWorld::addCollisionObject(body,
-                                         collisionFilterGroup,
-                                         collisionFilterMask);
+	m_softBodies.push_back(body);
+
+	// Set the soft body solver that will deal with this body
+	// to be the world's solver
+	body->setSoftBodySolver(m_deformableBodySolver);
+
+	btCollisionWorld::addCollisionObject(body,
+										 collisionFilterGroup,
+										 collisionFilterMask);
 }
 
 void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
-    BT_PROFILE("predictUnconstraintMotion");
-    btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep);
-    m_deformableBodySolver->predictMotion(timeStep);
+	BT_PROFILE("predictUnconstraintMotion");
+	btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep);
+	m_deformableBodySolver->predictMotion(timeStep);
 }
 
 void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
 {
-    m_internalTime += timeStep;
-    m_deformableBodySolver->setImplicit(m_implicit);
-    m_deformableBodySolver->setLineSearch(m_lineSearch);
-    m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
-    btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
-    dispatchInfo.m_timeStep = timeStep;
-    dispatchInfo.m_stepCount = 0;
-    dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
-    btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
-    if (m_useProjection)
-    {
-        m_deformableBodySolver->m_useProjection = true;
-//        m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true;
-        m_deformableBodySolver->m_objective->m_preconditioner =  m_deformableBodySolver->m_objective->m_massPreconditioner;
-    }
-    else
-    {
-        m_deformableBodySolver->m_objective->m_preconditioner =  m_deformableBodySolver->m_objective->m_KKTPreconditioner;
-    }
-        
+	m_internalTime += timeStep;
+	m_deformableBodySolver->setImplicit(m_implicit);
+	m_deformableBodySolver->setLineSearch(m_lineSearch);
+	m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
+	btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
+	dispatchInfo.m_timeStep = timeStep;
+	dispatchInfo.m_stepCount = 0;
+	dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
+	btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
+	if (m_useProjection)
+	{
+		m_deformableBodySolver->m_useProjection = true;
+		m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true;
+		m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner;
+	}
+	else
+	{
+		m_deformableBodySolver->m_useProjection = false;
+		m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false;
+		m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner;
+	}
 }
 
-
 void btDeformableMultiBodyDynamicsWorld::debugDrawWorld()
 {
-
 	btMultiBodyDynamicsWorld::debugDrawWorld();
 
 	for (int i = 0; i < getSoftBodyArray().size(); i++)
@@ -556,253 +555,260 @@ void btDeformableMultiBodyDynamicsWorld::debugDrawWorld()
 			btSoftBodyHelpers::Draw(psb, getDebugDrawer(), getDrawFlags());
 		}
 	}
-
-	
 }
 
 void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep)
 {
-    // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
-    // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
-    // when there are multiple substeps
-    btMultiBodyDynamicsWorld::applyGravity();
-    // integrate rigid body gravity
-    for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
-    {
-        btRigidBody* rb = m_nonStaticRigidBodies[i];
-        rb->integrateVelocities(timeStep);
-    }
-    
-    // integrate multibody gravity
-    {
-        forwardKinematics();
-        clearMultiBodyConstraintForces();
-        {
-            for (int i = 0; i < this->m_multiBodies.size(); i++)
-            {
-                btMultiBody* bod = m_multiBodies[i];
-                
-                bool isSleeping = false;
-                
-                if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-                {
-                    isSleeping = true;
-                }
-                for (int b = 0; b < bod->getNumLinks(); b++)
-                {
-                    if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
-                        isSleeping = true;
-                }
-                
-                if (!isSleeping)
-                {
-                    m_scratch_r.resize(bod->getNumLinks() + 1);
-                    m_scratch_v.resize(bod->getNumLinks() + 1);
-                    m_scratch_m.resize(bod->getNumLinks() + 1);
-                    bool isConstraintPass = false;
-                    {
-                        if (!bod->isUsingRK4Integration())
-                        {
-                            bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep,
-                                                                                      m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
-                                                                                      getSolverInfo().m_jointFeedbackInWorldSpace,
-                                                                                      getSolverInfo().m_jointFeedbackInJointFrame);
-                        }
-                        else
-                        {
-                            btAssert(" RK4Integration is not supported" );
-                        }
-                    }
-                }
-            }
-        }
-    }
-    clearGravity();
+	// Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
+	// so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
+	// when there are multiple substeps
+	btMultiBodyDynamicsWorld::applyGravity();
+	// integrate rigid body gravity
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
+	{
+		btRigidBody* rb = m_nonStaticRigidBodies[i];
+		rb->integrateVelocities(timeStep);
+	}
+
+	// integrate multibody gravity
+	{
+		forwardKinematics();
+		clearMultiBodyConstraintForces();
+		{
+			for (int i = 0; i < this->m_multiBodies.size(); i++)
+			{
+				btMultiBody* bod = m_multiBodies[i];
+
+				bool isSleeping = false;
+
+				if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+				{
+					isSleeping = true;
+				}
+				for (int b = 0; b < bod->getNumLinks(); b++)
+				{
+					if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+						isSleeping = true;
+				}
+
+				if (!isSleeping)
+				{
+					m_scratch_r.resize(bod->getNumLinks() + 1);
+					m_scratch_v.resize(bod->getNumLinks() + 1);
+					m_scratch_m.resize(bod->getNumLinks() + 1);
+					bool isConstraintPass = false;
+					{
+						if (!bod->isUsingRK4Integration())
+						{
+							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep,
+																					  m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+																					  getSolverInfo().m_jointFeedbackInWorldSpace,
+																					  getSolverInfo().m_jointFeedbackInJointFrame);
+						}
+						else
+						{
+							btAssert(" RK4Integration is not supported");
+						}
+					}
+				}
+			}
+		}
+	}
+	clearGravity();
 }
 
 void btDeformableMultiBodyDynamicsWorld::clearGravity()
 {
-    BT_PROFILE("btMultiBody clearGravity");
-    // clear rigid body gravity
-    for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
-    {
-        btRigidBody* body = m_nonStaticRigidBodies[i];
-        if (body->isActive())
-        {
-            body->clearGravity();
-        }
-    }
-    // clear multibody gravity
-    for (int i = 0; i < this->m_multiBodies.size(); i++)
-    {
-        btMultiBody* bod = m_multiBodies[i];
-        
-        bool isSleeping = false;
-        
-        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-        {
-            isSleeping = true;
-        }
-        for (int b = 0; b < bod->getNumLinks(); b++)
-        {
-            if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
-                isSleeping = true;
-        }
-        
-        if (!isSleeping)
-        {
-            bod->addBaseForce(-m_gravity * bod->getBaseMass());
-            
-            for (int j = 0; j < bod->getNumLinks(); ++j)
-            {
-                bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j));
-            }
-        }
-    }
+	BT_PROFILE("btMultiBody clearGravity");
+	// clear rigid body gravity
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
+	{
+		btRigidBody* body = m_nonStaticRigidBodies[i];
+		if (body->isActive())
+		{
+			body->clearGravity();
+		}
+	}
+	// clear multibody gravity
+	for (int i = 0; i < this->m_multiBodies.size(); i++)
+	{
+		btMultiBody* bod = m_multiBodies[i];
+
+		bool isSleeping = false;
+
+		if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+		{
+			isSleeping = true;
+		}
+		for (int b = 0; b < bod->getNumLinks(); b++)
+		{
+			if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+				isSleeping = true;
+		}
+
+		if (!isSleeping)
+		{
+			bod->addBaseForce(-m_gravity * bod->getBaseMass());
+
+			for (int j = 0; j < bod->getNumLinks(); ++j)
+			{
+				bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j));
+			}
+		}
+	}
 }
 
 void btDeformableMultiBodyDynamicsWorld::beforeSolverCallbacks(btScalar timeStep)
 {
-    if (0 != m_internalTickCallback)
-    {
-        (*m_internalTickCallback)(this, timeStep);
-    }
-    
-    if (0 != m_solverCallback)
-    {
-        (*m_solverCallback)(m_internalTime, this);
-    }
+	if (0 != m_internalTickCallback)
+	{
+		(*m_internalTickCallback)(this, timeStep);
+	}
+
+	if (0 != m_solverCallback)
+	{
+		(*m_solverCallback)(m_internalTime, this);
+	}
 }
 
 void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
 {
-    if (0 != m_solverCallback)
-    {
-        (*m_solverCallback)(m_internalTime, this);
-    }
+	if (0 != m_solverCallback)
+	{
+		(*m_solverCallback)(m_internalTime, this);
+	}
 }
 
 void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force)
 {
-    btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
-    bool added = false;
-    for (int i = 0; i < forces.size(); ++i)
-    {
-        if (forces[i]->getForceType() == force->getForceType())
-        {
-            forces[i]->addSoftBody(psb);
-            added = true;
-            break;
-        }
-    }
-    if (!added)
-    {
-        force->addSoftBody(psb);
-        force->setIndices(m_deformableBodySolver->m_objective->getIndices());
-        forces.push_back(force);
-    }
+	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+	bool added = false;
+	for (int i = 0; i < forces.size(); ++i)
+	{
+		if (forces[i]->getForceType() == force->getForceType())
+		{
+			forces[i]->addSoftBody(psb);
+			added = true;
+			break;
+		}
+	}
+	if (!added)
+	{
+		force->addSoftBody(psb);
+		force->setIndices(m_deformableBodySolver->m_objective->getIndices());
+		forces.push_back(force);
+	}
 }
 
 void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force)
 {
-    btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
-    int removed_index = -1;
-    for (int i = 0; i < forces.size(); ++i)
-    {
-        if (forces[i]->getForceType() == force->getForceType())
-        {
-            forces[i]->removeSoftBody(psb);
-            if (forces[i]->m_softBodies.size() == 0)
-                removed_index = i;
-            break;
-        }
-    }
-    if (removed_index >= 0)
-        forces.removeAtIndex(removed_index);
+	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+	int removed_index = -1;
+	for (int i = 0; i < forces.size(); ++i)
+	{
+		if (forces[i]->getForceType() == force->getForceType())
+		{
+			forces[i]->removeSoftBody(psb);
+			if (forces[i]->m_softBodies.size() == 0)
+				removed_index = i;
+			break;
+		}
+	}
+	if (removed_index >= 0)
+		forces.removeAtIndex(removed_index);
+}
+
+void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb)
+{
+	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+	for (int i = 0; i < forces.size(); ++i)
+	{
+		forces[i]->removeSoftBody(psb);
+	}
 }
 
 void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
 {
-    m_softBodies.remove(body);
-    btCollisionWorld::removeCollisionObject(body);
-    // force a reinitialize so that node indices get updated.
-    m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1));
+	removeSoftBodyForce(body);
+	m_softBodies.remove(body);
+	btCollisionWorld::removeCollisionObject(body);
+	// force a reinitialize so that node indices get updated.
+	m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1));
 }
 
 void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
 {
-    btSoftBody* body = btSoftBody::upcast(collisionObject);
-    if (body)
-        removeSoftBody(body);
-    else
-        btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
+	btSoftBody* body = btSoftBody::upcast(collisionObject);
+	if (body)
+		removeSoftBody(body);
+	else
+		btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
 }
 
-
 int btDeformableMultiBodyDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
 {
-    startProfiling(timeStep);
-    
-    int numSimulationSubSteps = 0;
-    
-    if (maxSubSteps)
-    {
-        //fixed timestep with interpolation
-        m_fixedTimeStep = fixedTimeStep;
-        m_localTime += timeStep;
-        if (m_localTime >= fixedTimeStep)
-        {
-            numSimulationSubSteps = int(m_localTime / fixedTimeStep);
-            m_localTime -= numSimulationSubSteps * fixedTimeStep;
-        }
-    }
-    else
-    {
-        //variable timestep
-        fixedTimeStep = timeStep;
-        m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep;
-        m_fixedTimeStep = 0;
-        if (btFuzzyZero(timeStep))
-        {
-            numSimulationSubSteps = 0;
-            maxSubSteps = 0;
-        }
-        else
-        {
-            numSimulationSubSteps = 1;
-            maxSubSteps = 1;
-        }
-    }
-    
-    //process some debugging flags
-    if (getDebugDrawer())
-    {
-        btIDebugDraw* debugDrawer = getDebugDrawer();
-        gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
-    }
-    if (numSimulationSubSteps)
-    {
-        //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
-        int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
-        
-        saveKinematicState(fixedTimeStep * clampedSimulationSteps);
-        
-        for (int i = 0; i < clampedSimulationSteps; i++)
-        {
-            internalSingleStepSimulation(fixedTimeStep);
-            synchronizeMotionStates();
-        }
-    }
-    else
-    {
-        synchronizeMotionStates();
-    }
-    
-    clearForces();
-    
+	startProfiling(timeStep);
+
+	int numSimulationSubSteps = 0;
+
+	if (maxSubSteps)
+	{
+		//fixed timestep with interpolation
+		m_fixedTimeStep = fixedTimeStep;
+		m_localTime += timeStep;
+		if (m_localTime >= fixedTimeStep)
+		{
+			numSimulationSubSteps = int(m_localTime / fixedTimeStep);
+			m_localTime -= numSimulationSubSteps * fixedTimeStep;
+		}
+	}
+	else
+	{
+		//variable timestep
+		fixedTimeStep = timeStep;
+		m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep;
+		m_fixedTimeStep = 0;
+		if (btFuzzyZero(timeStep))
+		{
+			numSimulationSubSteps = 0;
+			maxSubSteps = 0;
+		}
+		else
+		{
+			numSimulationSubSteps = 1;
+			maxSubSteps = 1;
+		}
+	}
+
+	//process some debugging flags
+	if (getDebugDrawer())
+	{
+		btIDebugDraw* debugDrawer = getDebugDrawer();
+		gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
+	}
+	if (numSimulationSubSteps)
+	{
+		//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
+		int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
+
+		saveKinematicState(fixedTimeStep * clampedSimulationSteps);
+
+		for (int i = 0; i < clampedSimulationSteps; i++)
+		{
+			internalSingleStepSimulation(fixedTimeStep);
+			synchronizeMotionStates();
+		}
+	}
+	else
+	{
+		synchronizeMotionStates();
+	}
+
+	clearForces();
+
 #ifndef BT_NO_PROFILE
-    CProfileManager::Increment_Frame_Counter();
+	CProfileManager::Increment_Frame_Counter();
 #endif  //BT_NO_PROFILE
-    
-    return numSimulationSubSteps;
+
+	return numSimulationSubSteps;
 }

thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

@@ -36,185 +36,192 @@ typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
 
 class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
 {
-    typedef btAlignedObjectArray<btVector3> TVStack;
-    ///Solver classes that encapsulate multiple deformable bodies for solving
-    btDeformableBodySolver* m_deformableBodySolver;
-    btSoftBodyArray m_softBodies;
-    int m_drawFlags;
-    bool m_drawNodeTree;
-    bool m_drawFaceTree;
-    bool m_drawClusterTree;
-    btSoftBodyWorldInfo m_sbi;
-    btScalar m_internalTime;
-    int m_ccdIterations;
-    bool m_implicit;
-    bool m_lineSearch;
-    bool m_useProjection;
-    DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback;
-    
-    typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
-    btSolverCallback m_solverCallback;
-    
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	///Solver classes that encapsulate multiple deformable bodies for solving
+	btDeformableBodySolver* m_deformableBodySolver;
+	btSoftBodyArray m_softBodies;
+	int m_drawFlags;
+	bool m_drawNodeTree;
+	bool m_drawFaceTree;
+	bool m_drawClusterTree;
+	btSoftBodyWorldInfo m_sbi;
+	btScalar m_internalTime;
+	int m_ccdIterations;
+	bool m_implicit;
+	bool m_lineSearch;
+	bool m_useProjection;
+	DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback;
+
+	typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
+	btSolverCallback m_solverCallback;
+
 protected:
-    virtual void internalSingleStepSimulation(btScalar timeStep);
-    
-    virtual void integrateTransforms(btScalar timeStep);
-    
-    void positionCorrection(btScalar timeStep);
-    
-    void solveConstraints(btScalar timeStep);
-    
-    void updateActivationState(btScalar timeStep);
-    
-    void clearGravity();
-    
+	virtual void internalSingleStepSimulation(btScalar timeStep);
+
+	virtual void integrateTransforms(btScalar timeStep);
+
+	void positionCorrection(btScalar timeStep);
+
+	void solveConstraints(btScalar timeStep);
+
+	void updateActivationState(btScalar timeStep);
+
+	void clearGravity();
+
 public:
 	btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0);
 
-    virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
+	virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
 
 	virtual void debugDrawWorld();
 
-    void setSolverCallback(btSolverCallback cb)
-    {
-        m_solverCallback = cb;
-    }
-    
-    virtual ~btDeformableMultiBodyDynamicsWorld();
-    
-    virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
-    {
-        return (btMultiBodyDynamicsWorld*)(this);
-    }
-    
-    virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const
-    {
-        return (const btMultiBodyDynamicsWorld*)(this);
-    }
-    
-    virtual btDynamicsWorldType getWorldType() const
-    {
-        return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD;
-    }
-    
-    virtual void predictUnconstraintMotion(btScalar timeStep);
-    
-    virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
-    
-    btSoftBodyArray& getSoftBodyArray()
-    {
-        return m_softBodies;
-    }
-    
-    const btSoftBodyArray& getSoftBodyArray() const
-    {
-        return m_softBodies;
-    }
-    
-    btSoftBodyWorldInfo& getWorldInfo()
-    {
-        return m_sbi;
-    }
-    
-    const btSoftBodyWorldInfo& getWorldInfo() const
-    {
-        return m_sbi;
-    }
-    
-    void reinitialize(btScalar timeStep);
-    
-    void applyRigidBodyGravity(btScalar timeStep);
-    
-    void beforeSolverCallbacks(btScalar timeStep);
-    
-    void afterSolverCallbacks(btScalar timeStep);
-    
-    void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
-    
-    void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
-    
-    void removeSoftBody(btSoftBody* body);
-    
-    void removeCollisionObject(btCollisionObject* collisionObject);
-    
-    int getDrawFlags() const { return (m_drawFlags); }
-    void setDrawFlags(int f) { m_drawFlags = f; }
-    
-    void setupConstraints();
-    
-    void performDeformableCollisionDetection();
-    
-    void solveMultiBodyConstraints();
-    
-    void solveContactConstraints();
-    
-    void sortConstraints();
-    
-    void softBodySelfCollision();
-    
-    void setImplicit(bool implicit)
-    {
-        m_implicit = implicit;
-    }
-    
-    void setLineSearch(bool lineSearch)
-    {
-        m_lineSearch = lineSearch;
-    }
-    
-    void applyRepulsionForce(btScalar timeStep);
-    
-    void performGeometricCollisions(btScalar timeStep);
-    
-    struct btDeformableSingleRayCallback : public btBroadphaseRayCallback
-    {
-        btVector3 m_rayFromWorld;
-        btVector3 m_rayToWorld;
-        btTransform m_rayFromTrans;
-        btTransform m_rayToTrans;
-        btVector3 m_hitNormal;
-        
-        const btDeformableMultiBodyDynamicsWorld* m_world;
-        btCollisionWorld::RayResultCallback& m_resultCallback;
-        
-        btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
-        : m_rayFromWorld(rayFromWorld),
-        m_rayToWorld(rayToWorld),
-        m_world(world),
-        m_resultCallback(resultCallback)
-        {
-            m_rayFromTrans.setIdentity();
-            m_rayFromTrans.setOrigin(m_rayFromWorld);
-            m_rayToTrans.setIdentity();
-            m_rayToTrans.setOrigin(m_rayToWorld);
-            
-            btVector3 rayDir = (rayToWorld - rayFromWorld);
-            
-            rayDir.normalize();
-            ///what about division by zero? --> just set rayDirection[i] to INF/1e30
-            m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
-            m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
-            m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
-            m_signs[0] = m_rayDirectionInverse[0] < 0.0;
-            m_signs[1] = m_rayDirectionInverse[1] < 0.0;
-            m_signs[2] = m_rayDirectionInverse[2] < 0.0;
-            
-            m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
-        }
-        
-        virtual bool process(const btBroadphaseProxy* proxy)
-        {
-            ///terminate further ray tests, once the closestHitFraction reached zero
-            if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
-                return false;
-            
-            btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
-            
-            //only perform raycast if filterMask matches
-            if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
-            {
-                //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
-                //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+	void setSolverCallback(btSolverCallback cb)
+	{
+		m_solverCallback = cb;
+	}
+
+	virtual ~btDeformableMultiBodyDynamicsWorld();
+
+	virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
+	{
+		return (btMultiBodyDynamicsWorld*)(this);
+	}
+
+	virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const
+	{
+		return (const btMultiBodyDynamicsWorld*)(this);
+	}
+
+	virtual btDynamicsWorldType getWorldType() const
+	{
+		return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD;
+	}
+
+	virtual void predictUnconstraintMotion(btScalar timeStep);
+
+	virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
+
+	btSoftBodyArray& getSoftBodyArray()
+	{
+		return m_softBodies;
+	}
+
+	const btSoftBodyArray& getSoftBodyArray() const
+	{
+		return m_softBodies;
+	}
+
+	btSoftBodyWorldInfo& getWorldInfo()
+	{
+		return m_sbi;
+	}
+
+	const btSoftBodyWorldInfo& getWorldInfo() const
+	{
+		return m_sbi;
+	}
+
+	void reinitialize(btScalar timeStep);
+
+	void applyRigidBodyGravity(btScalar timeStep);
+
+	void beforeSolverCallbacks(btScalar timeStep);
+
+	void afterSolverCallbacks(btScalar timeStep);
+
+	void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
+
+	void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
+
+	void removeSoftBodyForce(btSoftBody* psb);
+
+	void removeSoftBody(btSoftBody* body);
+
+	void removeCollisionObject(btCollisionObject* collisionObject);
+
+	int getDrawFlags() const { return (m_drawFlags); }
+	void setDrawFlags(int f) { m_drawFlags = f; }
+
+	void setupConstraints();
+
+	void performDeformableCollisionDetection();
+
+	void solveMultiBodyConstraints();
+
+	void solveContactConstraints();
+
+	void sortConstraints();
+
+	void softBodySelfCollision();
+
+	void setImplicit(bool implicit)
+	{
+		m_implicit = implicit;
+	}
+
+	void setLineSearch(bool lineSearch)
+	{
+		m_lineSearch = lineSearch;
+	}
+
+	void setUseProjection(bool useProjection)
+	{
+		m_useProjection = useProjection;
+	}
+
+	void applyRepulsionForce(btScalar timeStep);
+
+	void performGeometricCollisions(btScalar timeStep);
+
+	struct btDeformableSingleRayCallback : public btBroadphaseRayCallback
+	{
+		btVector3 m_rayFromWorld;
+		btVector3 m_rayToWorld;
+		btTransform m_rayFromTrans;
+		btTransform m_rayToTrans;
+		btVector3 m_hitNormal;
+
+		const btDeformableMultiBodyDynamicsWorld* m_world;
+		btCollisionWorld::RayResultCallback& m_resultCallback;
+
+		btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
+			: m_rayFromWorld(rayFromWorld),
+			  m_rayToWorld(rayToWorld),
+			  m_world(world),
+			  m_resultCallback(resultCallback)
+		{
+			m_rayFromTrans.setIdentity();
+			m_rayFromTrans.setOrigin(m_rayFromWorld);
+			m_rayToTrans.setIdentity();
+			m_rayToTrans.setOrigin(m_rayToWorld);
+
+			btVector3 rayDir = (rayToWorld - rayFromWorld);
+
+			rayDir.normalize();
+			///what about division by zero? --> just set rayDirection[i] to INF/1e30
+			m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
+			m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
+			m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+			m_signs[0] = m_rayDirectionInverse[0] < 0.0;
+			m_signs[1] = m_rayDirectionInverse[1] < 0.0;
+			m_signs[2] = m_rayDirectionInverse[2] < 0.0;
+
+			m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
+		}
+
+		virtual bool process(const btBroadphaseProxy* proxy)
+		{
+			///terminate further ray tests, once the closestHitFraction reached zero
+			if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
+				return false;
+
+			btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
+
+			//only perform raycast if filterMask matches
+			if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
+			{
+				//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+				//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
 #if 0
 #ifdef RECALCULATE_AABB
                 btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
@@ -225,87 +232,85 @@ public:
                 const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
 #endif
 #endif
-                //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
-                //culling already done by broadphase
-                //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
-                {
-                    m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
-                                           collisionObject,
-                                           collisionObject->getCollisionShape(),
-                                           collisionObject->getWorldTransform(),
-                                           m_resultCallback);
-                }
-            }
-            return true;
-        }
-    };
-
-    
-    
-    void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
-    {
-        BT_PROFILE("rayTest");
-        /// use the broadphase to accelerate the search for objects, based on their aabb
-        /// and for each object with ray-aabb overlap, perform an exact ray test
-        btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
-        
+				//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
+				//culling already done by broadphase
+				//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
+				{
+					m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
+										   collisionObject,
+										   collisionObject->getCollisionShape(),
+										   collisionObject->getWorldTransform(),
+										   m_resultCallback);
+				}
+			}
+			return true;
+		}
+	};
+
+	void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
+	{
+		BT_PROFILE("rayTest");
+		/// use the broadphase to accelerate the search for objects, based on their aabb
+		/// and for each object with ray-aabb overlap, perform an exact ray test
+		btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
+
 #ifndef USE_BRUTEFORCE_RAYBROADPHASE
-        m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
+		m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
 #else
-        for (int i = 0; i < this->getNumCollisionObjects(); i++)
-        {
-            rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
-        }
+		for (int i = 0; i < this->getNumCollisionObjects(); i++)
+		{
+			rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
+		}
 #endif  //USE_BRUTEFORCE_RAYBROADPHASE
-    }
-    
-    void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
-                                                     btCollisionObject* collisionObject,
-                                                     const btCollisionShape* collisionShape,
-                                                     const btTransform& colObjWorldTransform,
-                                                     RayResultCallback& resultCallback) const
-    {
-        if (collisionShape->isSoftBody())
-        {
-            btSoftBody* softBody = btSoftBody::upcast(collisionObject);
-            if (softBody)
-            {
-                btSoftBody::sRayCast softResult;
-                if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
-                {
-                    if (softResult.fraction <= resultCallback.m_closestHitFraction)
-                    {
-                        btCollisionWorld::LocalShapeInfo shapeInfo;
-                        shapeInfo.m_shapePart = 0;
-                        shapeInfo.m_triangleIndex = softResult.index;
-                        // get the normal
-                        btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
-                        btVector3 normal = -rayDir;
-                        normal.normalize();
-                        {
-                            normal = softBody->m_faces[softResult.index].m_normal;
-                            if (normal.dot(rayDir) > 0)
-                            {
-                                // normal always point toward origin of the ray
-                                normal = -normal;
-                            }
-                        }
-                        
-                        btCollisionWorld::LocalRayResult rayResult(collisionObject,
-                                                                   &shapeInfo,
-                                                                   normal,
-                                                                   softResult.fraction);
-                        bool normalInWorldSpace = true;
-                        resultCallback.addSingleResult(rayResult, normalInWorldSpace);
-                    }
-                }
-            }
-        }
-        else
-        {
-            btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
-        }
-    }
+	}
+
+	void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
+					   btCollisionObject* collisionObject,
+					   const btCollisionShape* collisionShape,
+					   const btTransform& colObjWorldTransform,
+					   RayResultCallback& resultCallback) const
+	{
+		if (collisionShape->isSoftBody())
+		{
+			btSoftBody* softBody = btSoftBody::upcast(collisionObject);
+			if (softBody)
+			{
+				btSoftBody::sRayCast softResult;
+				if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
+				{
+					if (softResult.fraction <= resultCallback.m_closestHitFraction)
+					{
+						btCollisionWorld::LocalShapeInfo shapeInfo;
+						shapeInfo.m_shapePart = 0;
+						shapeInfo.m_triangleIndex = softResult.index;
+						// get the normal
+						btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
+						btVector3 normal = -rayDir;
+						normal.normalize();
+						{
+							normal = softBody->m_faces[softResult.index].m_normal;
+							if (normal.dot(rayDir) > 0)
+							{
+								// normal always point toward origin of the ray
+								normal = -normal;
+							}
+						}
+
+						btCollisionWorld::LocalRayResult rayResult(collisionObject,
+																   &shapeInfo,
+																   normal,
+																   softResult.fraction);
+						bool normalInWorldSpace = true;
+						resultCallback.addSingleResult(rayResult, normalInWorldSpace);
+					}
+				}
+			}
+		}
+		else
+		{
+			btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
+		}
+	}
 };
 
 #endif  //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H

thirdparty/bullet/BulletSoftBody/btDeformableNeoHookeanForce.h

@@ -23,30 +23,30 @@ subject to the following restrictions:
 class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
 {
 public:
-    typedef btAlignedObjectArray<btVector3> TVStack;
-	btScalar m_mu, m_lambda; // Lame Parameters
-	btScalar m_E, m_nu;  // Young's modulus and Poisson ratio
-    btScalar m_mu_damp, m_lambda_damp;
-    btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1)
-    {
-        btScalar damping = 0.05;
-        m_mu_damp = damping * m_mu;
-        m_lambda_damp = damping * m_lambda;
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btScalar m_mu, m_lambda;  // Lame Parameters
+	btScalar m_E, m_nu;       // Young's modulus and Poisson ratio
+	btScalar m_mu_damp, m_lambda_damp;
+	btDeformableNeoHookeanForce() : m_mu(1), m_lambda(1)
+	{
+		btScalar damping = 0.05;
+		m_mu_damp = damping * m_mu;
+		m_lambda_damp = damping * m_lambda;
 		updateYoungsModulusAndPoissonRatio();
-    }
-    
-    btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda)
-    {
-        m_mu_damp = damping * m_mu;
-        m_lambda_damp = damping * m_lambda;
+	}
+
+	btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05) : m_mu(mu), m_lambda(lambda)
+	{
+		m_mu_damp = damping * m_mu;
+		m_lambda_damp = damping * m_lambda;
 		updateYoungsModulusAndPoissonRatio();
-    }
+	}
 
 	void updateYoungsModulusAndPoissonRatio()
 	{
 		// conversion from Lame Parameters to Young's modulus and Poisson ratio
 		// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
-		m_E  = m_mu * (3*m_lambda + 2*m_mu)/(m_lambda + m_mu);
+		m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu);
 		m_nu = m_lambda * 0.5 / (m_mu + m_lambda);
 	}
 
@@ -55,21 +55,21 @@ public:
 		// 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));
+		m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu));
 	}
 
-    void setYoungsModulus(btScalar E)
-    {
+	void setYoungsModulus(btScalar E)
+	{
 		m_E = E;
 		updateLameParameters();
-    }
+	}
 
 	void setPoissonRatio(btScalar nu)
 	{
 		m_nu = nu;
 		updateLameParameters();
 	}
-	
+
 	void setDamping(btScalar damping)
 	{
 		m_mu_damp = damping * m_mu;
@@ -83,339 +83,338 @@ public:
 		updateYoungsModulusAndPoissonRatio();
 	}
 
-    virtual void addScaledForces(btScalar scale, TVStack& force)
-    {
-        addScaledDampingForce(scale, force);
-        addScaledElasticForce(scale, force);
-    }
-    
-    virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
-    {
-        addScaledElasticForce(scale, force);
-    }
-    
-    // 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)
-            return;
-        int numNodes = getNumNodes();
-        btAssert(numNodes <= force.size());
-        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];
-                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];
-                size_t id0 = node0->index;
-                size_t id1 = node1->index;
-                size_t id2 = node2->index;
-                size_t id3 = node3->index;
-                btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
-                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 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+	virtual void addScaledForces(btScalar scale, TVStack& force)
+	{
+		addScaledDampingForce(scale, force);
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+	{
+		addScaledElasticForce(scale, force);
+	}
+
+	// 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)
+			return;
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		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];
+				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];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
+				btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
+				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 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+
+				// damping force differential
+				btScalar scale1 = scale * tetra.m_element_measure;
+				force[id0] -= scale1 * df_on_node0;
+				force[id1] -= scale1 * df_on_node123.getColumn(0);
+				force[id2] -= scale1 * df_on_node123.getColumn(1);
+				force[id3] -= scale1 * df_on_node123.getColumn(2);
+			}
+		}
+	}
+
+	virtual double totalElasticEnergy(btScalar dt)
+	{
+		double energy = 0;
+		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_tetraScratches.size(); ++j)
+			{
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
+				energy += tetra.m_element_measure * elasticEnergyDensity(s);
+			}
+		}
+		return energy;
+	}
+
+	// The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
+	virtual double totalDampingEnergy(btScalar dt)
+	{
+		double energy = 0;
+		int sz = 0;
+		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_nodes.size(); ++j)
+			{
+				sz = btMax(sz, psb->m_nodes[j].index);
+			}
+		}
+		TVStack dampingForce;
+		dampingForce.resize(sz + 1);
+		for (int i = 0; i < dampingForce.size(); ++i)
+			dampingForce[i].setZero();
+		addScaledDampingForce(0.5, dampingForce);
+		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];
+				energy -= dampingForce[node.index].dot(node.m_v) / dt;
+			}
+		}
+		return energy;
+	}
+
+	double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
+	{
+		double density = 0;
+		density += m_mu * 0.5 * (s.m_trace - 3.);
+		density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda) * (s.m_J - 1. - 0.75 * m_mu / m_lambda);
+		density -= m_mu * 0.5 * log(s.m_trace + 1);
+		return density;
+	}
 
-                // damping force differential
-                btScalar scale1 = scale * tetra.m_element_measure;
-                force[id0] -= scale1 * df_on_node0;
-                force[id1] -= scale1 * df_on_node123.getColumn(0);
-                force[id2] -= scale1 * df_on_node123.getColumn(1);
-                force[id3] -= scale1 * df_on_node123.getColumn(2);
-            }
-        }
-    }
-    
-    virtual double totalElasticEnergy(btScalar dt)
-    {
-        double energy = 0;
-        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_tetraScratches.size(); ++j)
-            {
-                btSoftBody::Tetra& tetra = psb->m_tetras[j];
-                btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
-                energy += tetra.m_element_measure * elasticEnergyDensity(s);
-            }
-        }
-        return energy;
-    }
-    
-    // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
-    virtual double totalDampingEnergy(btScalar dt)
-    {
-        double energy = 0;
-        int sz = 0;
-        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_nodes.size(); ++j)
-            {
-                sz = btMax(sz, psb->m_nodes[j].index);
-            }
-        }
-        TVStack dampingForce;
-        dampingForce.resize(sz+1);
-        for (int i = 0; i < dampingForce.size(); ++i)
-            dampingForce[i].setZero();
-        addScaledDampingForce(0.5, dampingForce);
-        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];
-                energy -= dampingForce[node.index].dot(node.m_v) / dt;
-            }
-        }
-        return energy;
-    }
-    
-    double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
-    {
-        double density = 0;
-        density += m_mu * 0.5 * (s.m_trace - 3.);
-        density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda)* (s.m_J - 1. - 0.75 * m_mu / m_lambda);
-        density -= m_mu * 0.5 * log(s.m_trace+1);
-        return density;
-    }
-    
-    virtual void addScaledElasticForce(btScalar scale, TVStack& force)
-    {
-        int numNodes = getNumNodes();
-        btAssert(numNodes <= force.size());
-        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;
-            }
-            btScalar max_p = psb->m_cfg.m_maxStress;
-            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);
+	virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= force.size());
+		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;
+			}
+			btScalar max_p = psb->m_cfg.m_maxStress;
+			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);
 #ifdef USE_SVD
-                if (max_p > 0)
-                {
-                    // since we want to clamp the principal stress to max_p, we only need to
-                    // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
-                    btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
-                    if (trPTP > max_p * max_p)
-                    {
-                        btMatrix3x3 U, V;
-                        btVector3 sigma;
-                        singularValueDecomposition(P, U, sigma, V);
-                        sigma[0] = btMin(sigma[0], max_p);
-                        sigma[1] = btMin(sigma[1], max_p);
-                        sigma[2] = btMin(sigma[2], max_p);
-                        sigma[0] = btMax(sigma[0], -max_p);
-                        sigma[1] = btMax(sigma[1], -max_p);
-                        sigma[2] = btMax(sigma[2], -max_p);
-                        btMatrix3x3 Sigma;
-                        Sigma.setIdentity();
-                        Sigma[0][0] = sigma[0];
-                        Sigma[1][1] = sigma[1];
-                        Sigma[2][2] = sigma[2];
-                        P = U * Sigma * V.transpose();
-                    }
-                }
+				if (max_p > 0)
+				{
+					// since we want to clamp the principal stress to max_p, we only need to
+					// calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
+					btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
+					if (trPTP > max_p * max_p)
+					{
+						btMatrix3x3 U, V;
+						btVector3 sigma;
+						singularValueDecomposition(P, U, sigma, V);
+						sigma[0] = btMin(sigma[0], max_p);
+						sigma[1] = btMin(sigma[1], max_p);
+						sigma[2] = btMin(sigma[2], max_p);
+						sigma[0] = btMax(sigma[0], -max_p);
+						sigma[1] = btMax(sigma[1], -max_p);
+						sigma[2] = btMax(sigma[2], -max_p);
+						btMatrix3x3 Sigma;
+						Sigma.setIdentity();
+						Sigma[0][0] = sigma[0];
+						Sigma[1][1] = sigma[1];
+						Sigma[2][2] = sigma[2];
+						P = U * Sigma * V.transpose();
+					}
+				}
 #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();
-                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];
-                size_t id0 = node0->index;
-                size_t id1 = node1->index;
-                size_t id2 = node2->index;
-                size_t id3 = node3->index;
-                
-                // elastic force
-                btScalar scale1 = scale * tetra.m_element_measure;
-                force[id0] -= scale1 * force_on_node0;
-                force[id1] -= scale1 * force_on_node123.getColumn(0);
-                force[id2] -= scale1 * force_on_node123.getColumn(1);
-                force[id3] -= scale1 * force_on_node123.getColumn(2);
-            }
-        }
-    }
-    
-    // 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)
-            return;
-        int numNodes = getNumNodes();
-        btAssert(numNodes <= df.size());
-        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];
-                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];
-                size_t id0 = node0->index;
-                size_t id1 = node1->index;
-                size_t id2 = node2->index;
-                size_t id3 = node3->index;
-                btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
-                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 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
-                btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
+				//                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();
+				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];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
+
+				// elastic force
+				btScalar scale1 = scale * tetra.m_element_measure;
+				force[id0] -= scale1 * force_on_node0;
+				force[id1] -= scale1 * force_on_node123.getColumn(0);
+				force[id2] -= scale1 * force_on_node123.getColumn(1);
+				force[id3] -= scale1 * force_on_node123.getColumn(2);
+			}
+		}
+	}
+
+	// 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)
+			return;
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= df.size());
+		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];
+				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];
+				size_t id0 = node0->index;
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				size_t id3 = node3->index;
+				btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
+				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 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+				btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
+
+				// damping force differential
+				btScalar scale1 = scale * tetra.m_element_measure;
+				df[id0] -= scale1 * df_on_node0;
+				df[id1] -= scale1 * df_on_node123.getColumn(0);
+				df[id2] -= scale1 * df_on_node123.getColumn(1);
+				df[id3] -= scale1 * df_on_node123.getColumn(2);
+			}
+		}
+	}
+
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
+
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+	{
+		int numNodes = getNumNodes();
+		btAssert(numNodes <= df.size());
+		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];
+				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];
+				size_t id0 = node0->index;
+				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 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();
+				btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
+
+				// elastic force differential
+				btScalar scale1 = scale * tetra.m_element_measure;
+				df[id0] -= scale1 * df_on_node0;
+				df[id1] -= scale1 * df_on_node123.getColumn(0);
+				df[id2] -= scale1 * df_on_node123.getColumn(1);
+				df[id3] -= scale1 * df_on_node123.getColumn(2);
+			}
+		}
+	}
+
+	void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
+	{
+		btScalar c1 = (m_mu * (1. - 1. / (s.m_trace + 1.)));
+		btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
+		P = s.m_F * c1 + s.m_cofF * c2;
+	}
+
+	// Let P be the first piola stress.
+	// This function calculates the dP = dP/dF * dF
+	void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
+	{
+		btScalar c1 = m_mu * (1. - 1. / (s.m_trace + 1.));
+		btScalar c2 = (2. * m_mu) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace)));
+		btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF));
+		dP = dF * c1 + s.m_F * c2;
+		addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda * (s.m_J - 1.) - 0.75 * m_mu, dP);
+		dP += s.m_cofF * c3;
+	}
 
-                // damping force differential
-                btScalar scale1 = scale * tetra.m_element_measure;
-                df[id0] -= scale1 * df_on_node0;
-                df[id1] -= scale1 * df_on_node123.getColumn(0);
-                df[id2] -= scale1 * df_on_node123.getColumn(1);
-                df[id3] -= scale1 * df_on_node123.getColumn(2);
-            }
-        }
-    }
-    
-    virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){}
-    
-    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
-    {
-        int numNodes = getNumNodes();
-        btAssert(numNodes <= df.size());
-        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];
-                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];
-                size_t id0 = node0->index;
-                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 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();
-                btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
-                
-                // elastic force differential
-                btScalar scale1 = scale * tetra.m_element_measure;
-                df[id0] -= scale1 * df_on_node0;
-                df[id1] -= scale1 * df_on_node123.getColumn(0);
-                df[id2] -= scale1 * df_on_node123.getColumn(1);
-                df[id3] -= scale1 * df_on_node123.getColumn(2);
-            }
-        }
-    }
-    
-    void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
-    {
-        btScalar c1 = (m_mu * ( 1. - 1. / (s.m_trace + 1.)));
-        btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
-        P = s.m_F * c1 + s.m_cofF * c2;
-    }
-    
-    // Let P be the first piola stress.
-    // This function calculates the dP = dP/dF * dF
-    void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF,  btMatrix3x3& dP)
-    {
-        btScalar c1 = m_mu * ( 1. - 1. / (s.m_trace + 1.));
-        btScalar c2 = (2.*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace)));
-        btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF));
-        dP = dF * c1 + s.m_F * c2;
-        addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda*(s.m_J-1.) - 0.75*m_mu, dP);
-        dP += s.m_cofF * c3;
-    }
-    
-    // Let Q be the damping stress.
-    // This function calculates the dP = dQ/dF * dF
-    void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF,  btMatrix3x3& dP)
-    {
-        btScalar c1 = (m_mu_damp * ( 1. - 1. / (s.m_trace + 1.)));
-        btScalar c2 = ((2.*m_mu_damp) * DotProduct(s.m_F, dF) *(1./((1.+s.m_trace)*(1.+s.m_trace))));
-        btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF));
-        dP = dF * c1 + s.m_F * c2;
-        addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp*(s.m_J-1.) - 0.75*m_mu_damp, dP);
-        dP += s.m_cofF * c3;
-    }
-    
-    btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
-    {
-        btScalar ans = 0;
-        for (int i = 0; i < 3; ++i)
-        {
-            ans += A[i].dot(B[i]);
-        }
-        return ans;
-    }
-    
-    // Let C(A) be the cofactor of the matrix A
-    // Let H = the derivative of C(A) with respect to A evaluated at F = A
-    // This function calculates H*dF
-    void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M)
-    {
-        M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
-        M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
-        M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
-        M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
-        M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
-        M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
-        M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
-        M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
-        M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
-    }
-    
-    virtual btDeformableLagrangianForceType getForceType()
-    {
-        return BT_NEOHOOKEAN_FORCE;
-    }
-    
+	// Let Q be the damping stress.
+	// This function calculates the dP = dQ/dF * dF
+	void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
+	{
+		btScalar c1 = (m_mu_damp * (1. - 1. / (s.m_trace + 1.)));
+		btScalar c2 = ((2. * m_mu_damp) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace))));
+		btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF));
+		dP = dF * c1 + s.m_F * c2;
+		addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp * (s.m_J - 1.) - 0.75 * m_mu_damp, dP);
+		dP += s.m_cofF * c3;
+	}
+
+	btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
+	{
+		btScalar ans = 0;
+		for (int i = 0; i < 3; ++i)
+		{
+			ans += A[i].dot(B[i]);
+		}
+		return ans;
+	}
+
+	// Let C(A) be the cofactor of the matrix A
+	// Let H = the derivative of C(A) with respect to A evaluated at F = A
+	// This function calculates H*dF
+	void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M)
+	{
+		M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
+		M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
+		M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
+		M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
+		M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
+		M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
+		M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
+		M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
+		M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
+	}
+
+	virtual btDeformableLagrangianForceType getForceType()
+	{
+		return BT_NEOHOOKEAN_FORCE;
+	}
 };
 #endif /* BT_NEOHOOKEAN_H */

thirdparty/bullet/BulletSoftBody/btKrylovSolver.h

@@ -0,0 +1,107 @@
+/*
+ Written by Xuchen Han <[email protected]>
+ 
+ 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

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

thirdparty/bullet/BulletSoftBody/btSoftBody.h

@@ -35,7 +35,7 @@ subject to the following restrictions:
 //#else
 #define btSoftBodyData btSoftBodyFloatData
 #define btSoftBodyDataName "btSoftBodyFloatData"
-static const btScalar  OVERLAP_REDUCTION_FACTOR = 0.1;
+static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1;
 static unsigned long seed = 243703;
 //#endif //BT_USE_DOUBLE_PRECISION
 
@@ -171,10 +171,10 @@ public:
 			CL_SELF = 0x0040,  ///Cluster soft body self collision
 			VF_DD = 0x0080,    ///Vertex vs face soft vs soft handling
 
-			RVDFmask = 0x0f00, /// Rigid versus deformable face mask
-			SDF_RDF = 0x0100,  /// GJK based Rigid vs. deformable face
-			SDF_MDF = 0x0200,  /// GJK based Multibody vs. deformable face
-            SDF_RDN = 0x0400,  /// SDF based Rigid vs. deformable node
+			RVDFmask = 0x0f00,  /// Rigid versus deformable face mask
+			SDF_RDF = 0x0100,   /// GJK based Rigid vs. deformable face
+			SDF_MDF = 0x0200,   /// GJK based Multibody vs. deformable face
+			SDF_RDN = 0x0400,   /// SDF based Rigid vs. deformable node
 			/* presets	*/
 			Default = SDF_RS,
 			END
@@ -226,7 +226,7 @@ public:
 		const btCollisionObject* m_colObj; /* Rigid body			*/
 		btVector3 m_normal;                /* Outward normal		*/
 		btScalar m_offset;                 /* Offset from origin	*/
-        btVector3 m_bary;                  /* Barycentric weights for faces */
+		btVector3 m_bary;                  /* Barycentric weights for faces */
 	};
 
 	/* sMedium		*/
@@ -258,20 +258,29 @@ public:
 		Material* m_material;  // Material
 	};
 	/* Node			*/
+	struct RenderNode
+	{
+		btVector3 m_x;
+		btVector3 m_uv1;
+		btVector3 m_normal;
+	};
 	struct Node : Feature
 	{
 		btVector3 m_x;       // Position
 		btVector3 m_q;       // Previous step position/Test position
 		btVector3 m_v;       // Velocity
-        btVector3 m_vn;      // Previous step velocity
+		btVector3 m_vn;      // Previous step velocity
 		btVector3 m_f;       // Force accumulator
 		btVector3 m_n;       // Normal
 		btScalar m_im;       // 1/mass
 		btScalar m_area;     // Area
 		btDbvtNode* m_leaf;  // Leaf data
-		btScalar m_penetration;   // depth of penetration
+		int m_constrained;   // depth of penetration
 		int m_battach : 1;   // Attached
-        int index;
+		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,40 +296,47 @@ public:
 
 		BT_DECLARE_ALIGNED_ALLOCATOR();
 	};
+	struct RenderFace
+	{
+		RenderNode* m_n[3];          // Node pointers
+	};
+
 	/* Face			*/
 	struct Face : Feature
 	{
-		Node* m_n[3];        // Node pointers
-		btVector3 m_normal;  // Normal
-		btScalar m_ra;       // Rest area
-		btDbvtNode* m_leaf;  // Leaf data
-        btVector4 m_pcontact; // barycentric weights of the persistent contact
-        btVector3 m_n0, m_n1, m_vn;
-        int m_index;
+		Node* m_n[3];          // Node pointers
+		btVector3 m_normal;    // Normal
+		btScalar m_ra;         // Rest area
+		btDbvtNode* m_leaf;    // Leaf data
+		btVector4 m_pcontact;  // barycentric weights of the persistent contact
+		btVector3 m_n0, m_n1, m_vn;
+		int m_index;
 	};
 	/* Tetra		*/
 	struct Tetra : Feature
 	{
-		Node* m_n[4];        // Node pointers
-		btScalar m_rv;       // Rest volume
-		btDbvtNode* m_leaf;  // Leaf data
-		btVector3 m_c0[4];   // gradients
-		btScalar m_c1;       // (4*kVST)/(im0+im1+im2+im3)
-		btScalar m_c2;       // m_c1/sum(|g0..3|^2)
-        btMatrix3x3 m_Dm_inverse; // rest Dm^-1
-        btMatrix3x3 m_F;
-        btScalar m_element_measure;
+		Node* m_n[4];              // Node pointers
+		btScalar m_rv;             // Rest volume
+		btDbvtNode* m_leaf;        // Leaf data
+		btVector3 m_c0[4];         // gradients
+		btScalar m_c1;             // (4*kVST)/(im0+im1+im2+im3)
+		btScalar m_c2;             // m_c1/sum(|g0..3|^2)
+		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  */
+	struct TetraScratch
+	{
+		btMatrix3x3 m_F;           // deformation gradient F
+		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
 	};
-    
-    /*  TetraScratch  */
-    struct TetraScratch
-    {
-        btMatrix3x3 m_F;                // deformation gradient F
-        btScalar m_trace;               // trace of F^T * F
-        btScalar m_J;                   // det(F)
-        btMatrix3x3 m_cofF;             // cofactor of F
-    };
-    
+
 	/* RContact		*/
 	struct RContact
 	{
@@ -331,67 +347,68 @@ public:
 		btScalar m_c2;     // ima*dt
 		btScalar m_c3;     // Friction
 		btScalar m_c4;     // Hardness
-        
-        // jacobians and unit impulse responses for multibody
-        btMultiBodyJacobianData jacobianData_normal;
-        btMultiBodyJacobianData jacobianData_t1;
-        btMultiBodyJacobianData jacobianData_t2;
-        btVector3 t1;
-        btVector3 t2;
+
+		// jacobians and unit impulse responses for multibody
+		btMultiBodyJacobianData jacobianData_normal;
+		btMultiBodyJacobianData jacobianData_t1;
+		btMultiBodyJacobianData jacobianData_t2;
+		btVector3 t1;
+		btVector3 t2;
 	};
-    
-    class DeformableRigidContact
-    {
-    public:
-        sCti m_cti;        // Contact infos
-        btMatrix3x3 m_c0;  // Impulse matrix
-        btVector3 m_c1;    // Relative anchor
-        btScalar m_c2;     // inverse mass of node/face
-        btScalar m_c3;     // Friction
-        btScalar m_c4;     // Hardness
-        
-        // jacobians and unit impulse responses for multibody
-        btMultiBodyJacobianData jacobianData_normal;
-        btMultiBodyJacobianData jacobianData_t1;
-        btMultiBodyJacobianData jacobianData_t2;
-        btVector3 t1;
-        btVector3 t2;
-    };
-    
-    class DeformableNodeRigidContact : public DeformableRigidContact
-    {
-    public:
-        Node* m_node;      // Owner node
-    };
-    
-    class DeformableNodeRigidAnchor : public DeformableNodeRigidContact
-    {
-    public:
-        btVector3 m_local;    // Anchor position in body space
-    };
-    
-    class DeformableFaceRigidContact : public DeformableRigidContact
-    {
-    public:
-        Face* m_face;                   // Owner face
-        btVector3 m_contactPoint;       // Contact point
-        btVector3 m_bary;               // Barycentric weights
-        btVector3 m_weights;            // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
-    };
-    
-    struct DeformableFaceNodeContact
-    {
-        Node* m_node;         // Node
-        Face* m_face;         // Face
-        btVector3 m_bary;     // Barycentric weights
-        btVector3 m_weights;  // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
-        btVector3 m_normal;   // Normal
-        btScalar m_margin;    // Margin
-        btScalar m_friction;  // Friction
-        btScalar m_imf;       // inverse mass of the face at contact point
-        btScalar m_c0;        // scale of the impulse matrix;
-    };
-    
+
+	class DeformableRigidContact
+	{
+	public:
+		sCti m_cti;        // Contact infos
+		btMatrix3x3 m_c0;  // Impulse matrix
+		btVector3 m_c1;    // Relative anchor
+		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;
+		btMultiBodyJacobianData jacobianData_t1;
+		btMultiBodyJacobianData jacobianData_t2;
+		btVector3 t1;
+		btVector3 t2;
+	};
+
+	class DeformableNodeRigidContact : public DeformableRigidContact
+	{
+	public:
+		Node* m_node;  // Owner node
+	};
+
+	class DeformableNodeRigidAnchor : public DeformableNodeRigidContact
+	{
+	public:
+		btVector3 m_local;  // Anchor position in body space
+	};
+
+	class DeformableFaceRigidContact : public DeformableRigidContact
+	{
+	public:
+		Face* m_face;              // Owner face
+		btVector3 m_contactPoint;  // Contact point
+		btVector3 m_bary;          // Barycentric weights
+		btVector3 m_weights;       // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
+	};
+
+	struct DeformableFaceNodeContact
+	{
+		Node* m_node;         // Node
+		Face* m_face;         // Face
+		btVector3 m_bary;     // Barycentric weights
+		btVector3 m_weights;  // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
+		btVector3 m_normal;   // Normal
+		btScalar m_margin;    // Margin
+		btScalar m_friction;  // Friction
+		btScalar m_imf;       // inverse mass of the face at contact point
+		btScalar m_c0;        // scale of the impulse matrix;
+	};
+
 	/* SContact		*/
 	struct SContact
 	{
@@ -718,19 +735,19 @@ public:
 		tVSolverArray m_vsequence;  // Velocity solvers sequence
 		tPSolverArray m_psequence;  // Position solvers sequence
 		tPSolverArray m_dsequence;  // Drift solvers sequence
-        btScalar drag;           // deformable air drag
-        btScalar m_maxStress;       // Maximum principle first Piola stress
+		btScalar drag;              // deformable air drag
+		btScalar m_maxStress;       // Maximum principle first Piola stress
 	};
 	/* SolverState	*/
 	struct SolverState
 	{
 		//if you add new variables, always initialize them!
 		SolverState()
-			:sdt(0),
-			isdt(0),
-			velmrg(0),
-			radmrg(0),
-			updmrg(0)
+			: sdt(0),
+			  isdt(0),
+			  velmrg(0),
+			  radmrg(0),
+			  updmrg(0)
 		{
 		}
 		btScalar sdt;     // dt*timescale
@@ -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,40 +810,42 @@ 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;
-	tAnchorArray m_anchors;            // Anchors
-    btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors;
-	tRContactArray m_rcontacts;        // Rigid contacts
-    btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
-    btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
-    btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
-	tSContactArray m_scontacts;        // Soft contacts
-	tJointArray m_joints;              // Joints
-	tMaterialArray m_materials;        // Materials
-	btScalar m_timeacc;                // Time accumulator
-	btVector3 m_bounds[2];             // Spatial bounds
-	bool m_bUpdateRtCst;               // Update runtime constants
-	btDbvt m_ndbvt;                    // Nodes tree
-	btDbvt m_fdbvt;                    // Faces tree
-	btDbvntNode* m_fdbvnt;              // Faces tree with normals
-	btDbvt m_cdbvt;                    // Clusters tree
-	tClusterArray m_clusters;          // Clusters
-	btScalar m_dampingCoefficient;     // Damping Coefficient
+	btAlignedObjectArray<TetraScratch> m_tetraScratches;
+	btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
+	tAnchorArray m_anchors;  // Anchors
+	btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors;
+	tRContactArray m_rcontacts;  // Rigid contacts
+	btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
+	btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
+	btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
+	tSContactArray m_scontacts;     // Soft contacts
+	tJointArray m_joints;           // Joints
+	tMaterialArray m_materials;     // Materials
+	btScalar m_timeacc;             // Time accumulator
+	btVector3 m_bounds[2];          // Spatial bounds
+	bool m_bUpdateRtCst;            // Update runtime constants
+	btDbvt m_ndbvt;                 // Nodes tree
+	btDbvt m_fdbvt;                 // Faces tree
+	btDbvntNode* m_fdbvnt;          // Faces tree with normals
+	btDbvt m_cdbvt;                 // Clusters tree
+	tClusterArray m_clusters;       // Clusters
+	btScalar m_dampingCoefficient;  // Damping Coefficient
 	btScalar m_sleepingThreshold;
 	btScalar m_maxSpeedSquared;
-	btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
+	btAlignedObjectArray<btVector3> m_quads;  // quadrature points for collision detection
 	btScalar m_repulsionStiffness;
-    btAlignedObjectArray<btVector3> m_X;   // initial positions
+	btScalar m_gravityFactor;
+	bool m_cacheBarycenter;
+	btAlignedObjectArray<btVector3> m_X;  // initial positions
 
 	btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
 	btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
-	btAlignedObjectArray<btScalar> m_z; // vertical distance used in extrapolation
+	btAlignedObjectArray<btScalar> m_z;  // vertical distance used in extrapolation
 	bool m_useSelfCollision;
 	bool m_softSoftCollision;
 
@@ -856,11 +877,11 @@ public:
 	{
 		return m_worldInfo;
 	}
-    
-    void setDampingCoefficient(btScalar damping_coeff)
-    {
-        m_dampingCoefficient = damping_coeff;
-    }
+
+	void setDampingCoefficient(btScalar damping_coeff)
+	{
+		m_dampingCoefficient = damping_coeff;
+	}
 
 	///@todo: avoid internal softbody shape hack and move collision code to collision library
 	virtual void setCollisionShape(btCollisionShape* collisionShape)
@@ -921,11 +942,12 @@ public:
 					 Material* mat = 0);
 
 	/* Append anchor														*/
-    void appendDeformableAnchor(int node, btRigidBody* body);
-    void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link);
-    void appendAnchor(int node,
+	void appendDeformableAnchor(int node, btRigidBody* body);
+	void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link);
+	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());
@@ -976,10 +998,10 @@ public:
 	void setLinearVelocity(const btVector3& linVel);
 	/* Set the angular velocity of the center of mass                       */
 	void setAngularVelocity(const btVector3& angVel);
-    /* Get best fit rigid transform                                         */
-    btTransform getRigidTransform();
-    /* Transform to given pose                                              */
-    void transformTo(const btTransform& trs);
+	/* Get best fit rigid transform                                         */
+	btTransform getRigidTransform();
+	/* Transform to given pose                                              */
+	void transformTo(const btTransform& trs);
 	/* Transform															*/
 	void transform(const btTransform& trs);
 	/* Translate															*/
@@ -1068,11 +1090,11 @@ public:
 	/* defaultCollisionHandlers												*/
 	void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap);
 	void defaultCollisionHandler(btSoftBody* psb);
-    void setSelfCollision(bool useSelfCollision);
-    bool useSelfCollision();
-    void updateDeactivation(btScalar timeStep);
-    void setZeroVelocity();
-    bool wantsSleeping();
+	void setSelfCollision(bool useSelfCollision);
+	bool useSelfCollision();
+	void updateDeactivation(btScalar timeStep);
+	void setZeroVelocity();
+	bool wantsSleeping();
 
 	//
 	// Functionality to deal with new accelerated solvers.
@@ -1151,8 +1173,8 @@ public:
 	void rebuildNodeTree();
 	btVector3 evaluateCom() const;
 	bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
-    bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
-    bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
+	bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
+	bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
 	void updateNormals();
 	void updateBounds();
 	void updatePose();
@@ -1166,14 +1188,16 @@ public:
 	void solveClusters(btScalar sor);
 	void applyClusters(bool drift);
 	void dampClusters();
-    void setSpringStiffness(btScalar k);
-    void initializeDmInverse();
-    void updateDeformation();
-    void advanceDeformation();
+	void setSpringStiffness(btScalar k);
+	void setGravityFactor(btScalar gravFactor);
+	void setCacheBarycenter(bool cacheBarycenter);
+	void initializeDmInverse();
+	void updateDeformation();
+	void advanceDeformation();
 	void applyForces();
-    void setMaxStress(btScalar maxStress);
-    void interpolateRenderMesh();
-    void setCollisionQuadrature(int N);
+	void setMaxStress(btScalar maxStress);
+	void interpolateRenderMesh();
+	void setCollisionQuadrature(int N);
 	static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
 	static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);
 	static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti);
@@ -1182,14 +1206,15 @@ public:
 	static psolver_t getSolver(ePSolver::_ solver);
 	static vsolver_t getSolver(eVSolver::_ solver);
 	void geometricCollisionHandler(btSoftBody* psb);
-#define SAFE_EPSILON SIMD_EPSILON*100.0
+#define SAFE_EPSILON SIMD_EPSILON * 100.0
 	void updateNode(btDbvtNode* node, bool use_velocity, bool margin)
 	{
 		if (node->isleaf())
 		{
 			btSoftBody::Node* n = (btSoftBody::Node*)(node->data);
-			ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
-			btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			vol;
+			btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON;  // use user defined margin or margin for floating point precision
 			if (use_velocity)
 			{
 				btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v};
@@ -1207,38 +1232,40 @@ 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;
 		}
 	}
-	
-    void updateNodeTree(bool use_velocity, bool margin)
+
+	void updateNodeTree(bool use_velocity, bool margin)
 	{
 		if (m_ndbvt.m_root)
 			updateNode(m_ndbvt.m_root, use_velocity, margin);
 	}
 
-	template <class DBVTNODE> // btDbvtNode or btDbvntNode
+	template <class DBVTNODE>  // btDbvtNode or btDbvntNode
 	void updateFace(DBVTNODE* node, bool use_velocity, bool margin)
 	{
 		if (node->isleaf())
 		{
 			btSoftBody::Face* f = (btSoftBody::Face*)(node->data);
-			btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision
-			ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
+			btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON;  // use user defined margin or margin for floating point precision
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			vol;
 			if (use_velocity)
 			{
 				btVector3 points[6] = {f->m_n[0]->m_x, f->m_n[0]->m_x + m_sst.sdt * f->m_n[0]->m_v,
-					f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v,
-					f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v};
+									   f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v,
+									   f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v};
 				vol = btDbvtVolume::FromPoints(points, 6);
 			}
 			else
 			{
 				btVector3 points[3] = {f->m_n[0]->m_x,
-					f->m_n[1]->m_x,
-					f->m_n[2]->m_x};
+									   f->m_n[1]->m_x,
+									   f->m_n[2]->m_x};
 				vol = btDbvtVolume::FromPoints(points, 3);
 			}
 			vol.Expand(btVector3(pad, pad, pad));
@@ -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;
 		}
@@ -1271,7 +1299,7 @@ public:
 		return (a * coord.x() + b * coord.y() + c * coord.z());
 	}
 
-    void applyRepulsionForce(btScalar timeStep, bool applySpringForce)
+	void applyRepulsionForce(btScalar timeStep, bool applySpringForce)
 	{
 		btAlignedObjectArray<int> indices;
 		{
@@ -1297,58 +1325,60 @@ public:
 			const btVector3& n = c.m_normal;
 			btVector3 l = node->m_x - BaryEval(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, w);
 			btScalar d = c.m_margin - n.dot(l);
-			d = btMax(btScalar(0),d);
-			
+			d = btMax(btScalar(0), d);
+
 			const btVector3& va = node->m_v;
 			btVector3 vb = BaryEval(face->m_n[0]->m_v, face->m_n[1]->m_v, face->m_n[2]->m_v, w);
 			btVector3 vr = va - vb;
-			const btScalar vn = btDot(vr, n); // dn < 0 <==> opposing
+			const btScalar vn = btDot(vr, n);  // dn < 0 <==> opposing
 			if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep)
 				continue;
-			btVector3 vt = vr - vn*n;
+			btVector3 vt = vr - vn * n;
 			btScalar I = 0;
-			btScalar mass = node->m_im == 0 ? 0 : btScalar(1)/node->m_im;
+			btScalar mass = node->m_im == 0 ? 0 : btScalar(1) / node->m_im;
 			if (applySpringForce)
 				I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
 			if (vn < 0)
 				I += 0.5 * mass * vn;
-			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());
-			
-//             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)
+				face_penetration |= face->m_n[i]->m_constrained;
+			btScalar I_tilde = 2.0 * I / (1.0 + w.length2());
+
+			//             double the impulse if node or face is constrained.
+			if (face_penetration > 0 || node_penetration > 0)
+			{
+				I_tilde *= 2.0;
+			}
+			if (face_penetration <= 0)
 			{
 				for (int j = 0; j < 3; ++j)
-					face->m_n[j]->m_v += w[j]*n*I_tilde*node->m_im;
+					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;
+				node->m_v -= I_tilde * node->m_im * n;
 			}
-			
+
 			// apply frictional impulse
 			btScalar vt_norm = vt.safeNorm();
 			if (vt_norm > SIMD_EPSILON)
 			{
 				btScalar delta_vn = -2 * I * node->m_im;
 				btScalar mu = c.m_friction;
-				btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0))*vt_norm;
-				I = 0.5 * mass * (vt_norm-vt_new);
+				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());
-//                 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)
+				I_tilde = 2.0 * I / (1.0 + w.length2());
+				//                 double the impulse if node or face is constrained.
+				if (face_penetration > 0 || node_penetration > 0)
+					I_tilde *= 2.0;
+				if (face_penetration <= 0)
 				{
 					for (int j = 0; j < 3; ++j)
 						face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im;
 				}
-                if (face_penetration >= node_penetration)
+				if (node_penetration <= 0)
 				{
 					node->m_v -= I_tilde * node->m_im * vt;
 				}
@@ -1356,7 +1386,7 @@ public:
 		}
 	}
 	virtual int calculateSerializeBufferSize() const;
-  
+
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
 	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
 };

thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp

@@ -727,7 +727,7 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const
 										   int resy,
 										   int fixeds,
 										   bool gendiags,
-                                           btScalar perturbation)
+										   btScalar perturbation)
 {
 #define IDX(_x_, _y_) ((_y_)*rx + (_x_))
 	/* Create nodes	*/
@@ -747,12 +747,12 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const
 		for (int ix = 0; ix < rx; ++ix)
 		{
 			const btScalar tx = ix / (btScalar)(rx - 1);
-            btScalar pert = perturbation * btScalar(rand())/RAND_MAX;
-            btVector3 temp1 = py1;
-            temp1.setY(py1.getY() + pert);
-            btVector3 temp = py0;
-            pert = perturbation * btScalar(rand())/RAND_MAX;
-            temp.setY(py0.getY() + pert);
+			btScalar pert = perturbation * btScalar(rand()) / RAND_MAX;
+			btVector3 temp1 = py1;
+			temp1.setY(py1.getY() + pert);
+			btVector3 temp = py0;
+			pert = perturbation * btScalar(rand()) / RAND_MAX;
+			temp.setY(py0.getY() + pert);
 			x[IDX(ix, iy)] = lerp(temp, temp1, tx);
 			m[IDX(ix, iy)] = 1;
 		}
@@ -1233,9 +1233,9 @@ if(face&&face[0])
 			}
 		}
 	}
-    psb->initializeDmInverse();
-    psb->m_tetraScratches.resize(psb->m_tetras.size());
-    psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
+	psb->initializeDmInverse();
+	psb->m_tetraScratches.resize(psb->m_tetras.size());
+	psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
 	printf("Nodes:  %u\r\n", psb->m_nodes.size());
 	printf("Links:  %u\r\n", psb->m_links.size());
 	printf("Faces:  %u\r\n", psb->m_faces.size());
@@ -1245,61 +1245,62 @@ if(face&&face[0])
 
 btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file)
 {
-    std::ifstream fs;
-    fs.open(vtk_file);
-    btAssert(fs);
-    
-    typedef btAlignedObjectArray<int> Index;
-    std::string line;
-    btAlignedObjectArray<btVector3> X;
-    btVector3 position;
-    btAlignedObjectArray<Index> indices;
-    bool reading_points = false;
-    bool reading_tets = false;
-    size_t n_points = 0;
-    size_t n_tets = 0;
-    size_t x_count = 0;
-    size_t indices_count = 0;
-    while (std::getline(fs, line))
-    {
-        std::stringstream ss(line);
-        if (line.size() == (size_t)(0))
-        {
-        }
-        else if (line.substr(0, 6) == "POINTS")
-        {
-            reading_points = true;
-            reading_tets = false;
-            ss.ignore(128, ' '); // ignore "POINTS"
-            ss >> n_points;
-            X.resize(n_points);
-        }
-        else if (line.substr(0, 5) == "CELLS")
-        {
-            reading_points = false;
-            reading_tets = true;
-            ss.ignore(128, ' '); // ignore "CELLS"
-            ss >> n_tets;
-            indices.resize(n_tets);
-        }
-        else if (line.substr(0, 10) == "CELL_TYPES")
-        {
-            reading_points = false;
-            reading_tets = false;
-        }
-        else if (reading_points)
-        {
-            btScalar p;
-            ss >> p;
-            position.setX(p);
-            ss >> p;
-            position.setY(p);
-            ss >> p;
-            position.setZ(p);
-            X[x_count++] = position;
-        }
-        else if (reading_tets)
-        {
+	std::ifstream fs;
+	fs.open(vtk_file);
+	btAssert(fs);
+
+	typedef btAlignedObjectArray<int> Index;
+	std::string line;
+	btAlignedObjectArray<btVector3> X;
+	btVector3 position;
+	btAlignedObjectArray<Index> indices;
+	bool reading_points = false;
+	bool reading_tets = false;
+	size_t n_points = 0;
+	size_t n_tets = 0;
+	size_t x_count = 0;
+	size_t indices_count = 0;
+	while (std::getline(fs, line))
+	{
+		std::stringstream ss(line);
+		if (line.size() == (size_t)(0))
+		{
+		}
+		else if (line.substr(0, 6) == "POINTS")
+		{
+			reading_points = true;
+			reading_tets = false;
+			ss.ignore(128, ' ');  // ignore "POINTS"
+			ss >> n_points;
+			X.resize(n_points);
+		}
+		else if (line.substr(0, 5) == "CELLS")
+		{
+			reading_points = false;
+			reading_tets = true;
+			ss.ignore(128, ' ');  // ignore "CELLS"
+			ss >> n_tets;
+			indices.resize(n_tets);
+		}
+		else if (line.substr(0, 10) == "CELL_TYPES")
+		{
+			reading_points = false;
+			reading_tets = false;
+		}
+		else if (reading_points)
+		{
+			btScalar p;
+			ss >> p;
+			position.setX(p);
+			ss >> p;
+			position.setY(p);
+			ss >> p;
+			position.setZ(p);
+			//printf("v %f %f %f\n", position.getX(), position.getY(), position.getZ());
+			X[x_count++] = position;
+		}
+		else if (reading_tets)
+		{
 			int d;
 			ss >> d;
 			if (d != 4)
@@ -1308,317 +1309,355 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
 				fs.close();
 				return 0;
 			}
-            ss.ignore(128, ' '); // ignore "4"
-            Index tet;
-            tet.resize(4);
-            for (size_t i = 0; i < 4; i++)
-            {
-                ss >> tet[i];
-                printf("%d ", tet[i]);
-            }
-            printf("\n");
-            indices[indices_count++] = tet;
-        }
-    }
-    btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0);
-    
-    for (int i = 0; i < n_tets; ++i)
-    {
-        const Index& ni = indices[i];
-        psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
-        {
-            psb->appendLink(ni[0], ni[1], 0, true);
-            psb->appendLink(ni[1], ni[2], 0, true);
-            psb->appendLink(ni[2], ni[0], 0, true);
-            psb->appendLink(ni[0], ni[3], 0, true);
-            psb->appendLink(ni[1], ni[3], 0, true);
-            psb->appendLink(ni[2], ni[3], 0, true);
-        }
-    }
-    
-    
-    generateBoundaryFaces(psb);
-    psb->initializeDmInverse();
-    psb->m_tetraScratches.resize(psb->m_tetras.size());
-    psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
-    printf("Nodes:  %u\r\n", psb->m_nodes.size());
-    printf("Links:  %u\r\n", psb->m_links.size());
-    printf("Faces:  %u\r\n", psb->m_faces.size());
-    printf("Tetras: %u\r\n", psb->m_tetras.size());
-
-    fs.close();
-    return psb;
+			ss.ignore(128, ' ');  // ignore "4"
+			Index tet;
+			tet.resize(4);
+			for (size_t i = 0; i < 4; i++)
+			{
+				ss >> tet[i];
+				//printf("%d ", tet[i]);
+			}
+			//printf("\n");
+			indices[indices_count++] = tet;
+		}
+	}
+	btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0);
+
+	for (int i = 0; i < n_tets; ++i)
+	{
+		const Index& ni = indices[i];
+		psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
+		{
+			psb->appendLink(ni[0], ni[1], 0, true);
+			psb->appendLink(ni[1], ni[2], 0, true);
+			psb->appendLink(ni[2], ni[0], 0, true);
+			psb->appendLink(ni[0], ni[3], 0, true);
+			psb->appendLink(ni[1], ni[3], 0, true);
+			psb->appendLink(ni[2], ni[3], 0, true);
+		}
+	}
+
+	generateBoundaryFaces(psb);
+	psb->initializeDmInverse();
+	psb->m_tetraScratches.resize(psb->m_tetras.size());
+	psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
+	printf("Nodes:  %u\r\n", psb->m_nodes.size());
+	printf("Links:  %u\r\n", psb->m_links.size());
+	printf("Faces:  %u\r\n", psb->m_faces.size());
+	printf("Tetras: %u\r\n", psb->m_tetras.size());
+
+	fs.close();
+	return psb;
 }
 
 void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb)
 {
-    int counter = 0;
-    for (int i = 0; i < psb->m_nodes.size(); ++i)
-    {
-        psb->m_nodes[i].index = counter++;
-    }
-    typedef btAlignedObjectArray<int> Index;
-    btAlignedObjectArray<Index> indices;
-    indices.resize(psb->m_tetras.size());
-    for (int i = 0; i < indices.size(); ++i)
-    {
-        Index index;
-        index.push_back(psb->m_tetras[i].m_n[0]->index);
-        index.push_back(psb->m_tetras[i].m_n[1]->index);
-        index.push_back(psb->m_tetras[i].m_n[2]->index);
-        index.push_back(psb->m_tetras[i].m_n[3]->index);
-        indices[i] = index;
-    }
-    
-    std::map<std::vector<int>, std::vector<int> > dict;
-    for (int i = 0; i < indices.size(); ++i)
-    {
-        for (int j = 0; j < 4; ++j)
-        {
-            std::vector<int> f;
-            if (j == 0)
-            {
-                f.push_back(indices[i][1]);
-                f.push_back(indices[i][0]);
-                f.push_back(indices[i][2]);
-            }
-            if (j == 1)
-            {
-                f.push_back(indices[i][3]);
-                f.push_back(indices[i][0]);
-                f.push_back(indices[i][1]);
-            }
-            if (j == 2)
-            {
-                f.push_back(indices[i][3]);
-                f.push_back(indices[i][1]);
-                f.push_back(indices[i][2]);
-            }
-            if (j == 3)
-            {
-                f.push_back(indices[i][2]);
-                f.push_back(indices[i][0]);
-                f.push_back(indices[i][3]);
-            }
-            std::vector<int> f_sorted = f;
-            std::sort(f_sorted.begin(), f_sorted.end());
-            if (dict.find(f_sorted) != dict.end())
-            {
-                dict.erase(f_sorted);
-            }
-            else
-            {
-                dict.insert(std::make_pair(f_sorted, f));
-            }
-        }
-    }
-    
-    for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it)
-    {
-        std::vector<int> f = it->second;
-        psb->appendFace(f[0], f[1], f[2]);
-    }
+	int counter = 0;
+	for (int i = 0; i < psb->m_nodes.size(); ++i)
+	{
+		psb->m_nodes[i].index = counter++;
+	}
+	typedef btAlignedObjectArray<int> Index;
+	btAlignedObjectArray<Index> indices;
+	indices.resize(psb->m_tetras.size());
+	for (int i = 0; i < indices.size(); ++i)
+	{
+		Index index;
+		index.push_back(psb->m_tetras[i].m_n[0]->index);
+		index.push_back(psb->m_tetras[i].m_n[1]->index);
+		index.push_back(psb->m_tetras[i].m_n[2]->index);
+		index.push_back(psb->m_tetras[i].m_n[3]->index);
+		indices[i] = index;
+	}
+
+	std::map<std::vector<int>, std::vector<int> > dict;
+	for (int i = 0; i < indices.size(); ++i)
+	{
+		for (int j = 0; j < 4; ++j)
+		{
+			std::vector<int> f;
+			if (j == 0)
+			{
+				f.push_back(indices[i][1]);
+				f.push_back(indices[i][0]);
+				f.push_back(indices[i][2]);
+			}
+			if (j == 1)
+			{
+				f.push_back(indices[i][3]);
+				f.push_back(indices[i][0]);
+				f.push_back(indices[i][1]);
+			}
+			if (j == 2)
+			{
+				f.push_back(indices[i][3]);
+				f.push_back(indices[i][1]);
+				f.push_back(indices[i][2]);
+			}
+			if (j == 3)
+			{
+				f.push_back(indices[i][2]);
+				f.push_back(indices[i][0]);
+				f.push_back(indices[i][3]);
+			}
+			std::vector<int> f_sorted = f;
+			std::sort(f_sorted.begin(), f_sorted.end());
+			if (dict.find(f_sorted) != dict.end())
+			{
+				dict.erase(f_sorted);
+			}
+			else
+			{
+				dict.insert(std::make_pair(f_sorted, f));
+			}
+		}
+	}
+
+	for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it)
+	{
+		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);
-    for (int i = 0; i < psb->m_nodes.size(); ++i)
-    {
-        fs << "v";
-        for (int d = 0; d < 3; d++)
-        {
-             fs << " " << psb->m_nodes[i].m_x[d];
-        }
-        fs << "\n";
-    }
-    
-    for (int i = 0; i < psb->m_faces.size(); ++i)
-    {
-        fs << "f";
-        for (int n = 0; n < 3; n++)
-        {
-            fs << " " << psb->m_faces[i].m_n[n]->index + 1;
-        }
-        fs << "\n";
-    }
-    fs.close();
+	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";
+			for (int d = 0; d < 3; d++)
+			{
+				fs << " " << psb->m_nodes[i].m_x[d];
+			}
+			fs << "\n";
+		}
+
+		for (int i = 0; i < psb->m_faces.size(); ++i)
+		{
+			fs << "f";
+			for (int n = 0; n < 3; n++)
+			{
+				fs << " " << psb->m_faces[i].m_n[n]->index + 1;
+			}
+			fs << "\n";
+		}
+	}
+	fs.close();
 }
 
 void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb)
 {
-    std::ifstream fs_read;
-    fs_read.open(filename);
-    std::string line;
-    btVector3 pos;
-    btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces;
-    while (std::getline(fs_read, line))
-    {
-        std::stringstream ss(line);
-        if (line[0] == 'v')
-        {
-        }
-        else if (line[0] == 'f')
-        {
-            ss.ignore();
-            int id0, id1, id2;
-            ss >> id0;
-            ss >> id1;
-            ss >> id2;
-            btAlignedObjectArray<int> new_face;
-            new_face.push_back(id1);
-            new_face.push_back(id0);
-            new_face.push_back(id2);
-            additional_faces.push_back(new_face);
-        }
-    }
-    fs_read.close();
-
-    std::ofstream fs_write;
-    fs_write.open(filename, std::ios_base::app);
-    for (int i = 0; i < additional_faces.size(); ++i)
-    {
-        fs_write << "f";
-        for (int n = 0; n < 3; n++)
-        {
-            fs_write << " " << additional_faces[i][n];
-        }
-        fs_write << "\n";
-    }
-    fs_write.close();
+	std::ifstream fs_read;
+	fs_read.open(filename);
+	std::string line;
+	btVector3 pos;
+	btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces;
+	while (std::getline(fs_read, line))
+	{
+		std::stringstream ss(line);
+		if (line[0] == 'v')
+		{
+		}
+		else if (line[0] == 'f')
+		{
+			ss.ignore();
+			int id0, id1, id2;
+			ss >> id0;
+			ss >> id1;
+			ss >> id2;
+			btAlignedObjectArray<int> new_face;
+			new_face.push_back(id1);
+			new_face.push_back(id0);
+			new_face.push_back(id2);
+			additional_faces.push_back(new_face);
+		}
+	}
+	fs_read.close();
+
+	std::ofstream fs_write;
+	fs_write.open(filename, std::ios_base::app);
+	for (int i = 0; i < additional_faces.size(); ++i)
+	{
+		fs_write << "f";
+		for (int n = 0; n < 3; n++)
+		{
+			fs_write << " " << additional_faces[i][n];
+		}
+		fs_write << "\n";
+	}
+	fs_write.close();
 }
 
 // Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex
 void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary)
 {
-    btVector3 vap = p - a;
-    btVector3 vbp = p - b;
-    
-    btVector3 vab = b - a;
-    btVector3 vac = c - a;
-    btVector3 vad = d - a;
-    
-    btVector3 vbc = c - b;
-    btVector3 vbd = d - b;
-    btScalar va6 = (vbp.cross(vbd)).dot(vbc);
-    btScalar vb6 = (vap.cross(vac)).dot(vad);
-    btScalar vc6 = (vap.cross(vad)).dot(vab);
-    btScalar vd6 = (vap.cross(vab)).dot(vac);
-    btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad));
-    bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6);
+	btVector3 vap = p - a;
+	btVector3 vbp = p - b;
+
+	btVector3 vab = b - a;
+	btVector3 vac = c - a;
+	btVector3 vad = d - a;
+
+	btVector3 vbc = c - b;
+	btVector3 vbd = d - b;
+	btScalar va6 = (vbp.cross(vbd)).dot(vbc);
+	btScalar vb6 = (vap.cross(vac)).dot(vad);
+	btScalar vc6 = (vap.cross(vad)).dot(vab);
+	btScalar vd6 = (vap.cross(vab)).dot(vac);
+	btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad));
+	bary = btVector4(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6);
 }
 
 // Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0.
 void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary)
 {
-    btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
-    btScalar d00 = btDot(v0, v0);
-    btScalar d01 = btDot(v0, v1);
-    btScalar d11 = btDot(v1, v1);
-    btScalar d20 = btDot(v2, v0);
-    btScalar d21 = btDot(v2, v1);
-    btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01);
-    bary[1] = (d11 * d20 - d01 * d21) * invDenom;
-    bary[2] = (d00 * d21 - d01 * d20) * invDenom;
-    bary[0] = 1.0 - bary[1] - bary[2];
-    bary[3] = 0;
+	btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
+	btScalar d00 = btDot(v0, v0);
+	btScalar d01 = btDot(v0, v1);
+	btScalar d11 = btDot(v1, v1);
+	btScalar d20 = btDot(v2, v0);
+	btScalar d21 = btDot(v2, v1);
+	btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01);
+	bary[1] = (d11 * d20 - d01 * d21) * invDenom;
+	bary[2] = (d00 * d21 - d01 * d20) * invDenom;
+	bary[0] = 1.0 - bary[1] - bary[2];
+	bary[3] = 0;
 }
 
 // Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights
 // If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight
 void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
 {
-    psb->m_z.resize(0);
-    psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
-    psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
-    for (int i = 0; i < psb->m_renderNodes.size(); ++i)
-    {
-        const btVector3& p = psb->m_renderNodes[i].m_x;
-        btVector4 bary;
-        btVector4 optimal_bary;
-        btScalar min_bary_weight = -1e3;
-        btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
-        for (int j = 0; j < psb->m_tetras.size(); ++j)
-        {
-            const btSoftBody::Tetra& t = psb->m_tetras[j];
-            getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary);
-            btScalar new_min_bary_weight = bary[0];
-            for (int k = 1; k < 4; ++k)
-            {
-                new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
-            }
-            if (new_min_bary_weight > min_bary_weight)
-            {
-                btAlignedObjectArray<const btSoftBody::Node*> parents;
-                parents.push_back(t.m_n[0]);
-                parents.push_back(t.m_n[1]);
-                parents.push_back(t.m_n[2]);
-                parents.push_back(t.m_n[3]);
-                optimal_parents = parents;
-                optimal_bary = bary;
-                min_bary_weight = new_min_bary_weight;
-                // stop searching if p is inside the tetrahedron at hand
-                if (bary[0]>=0. && bary[1]>=0. && bary[2]>=0. && bary[3]>=0.)
-                {
-                    break;
-                }
-            }
-        }
-        psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
-        psb->m_renderNodesParents[i] = optimal_parents;
-    }
+	psb->m_z.resize(0);
+	psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
+	psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
+	for (int i = 0; i < psb->m_renderNodes.size(); ++i)
+	{
+		const btVector3& p = psb->m_renderNodes[i].m_x;
+		btVector4 bary;
+		btVector4 optimal_bary;
+		btScalar min_bary_weight = -1e3;
+		btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
+		for (int j = 0; j < psb->m_tetras.size(); ++j)
+		{
+			const btSoftBody::Tetra& t = psb->m_tetras[j];
+			getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary);
+			btScalar new_min_bary_weight = bary[0];
+			for (int k = 1; k < 4; ++k)
+			{
+				new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
+			}
+			if (new_min_bary_weight > min_bary_weight)
+			{
+				btAlignedObjectArray<const btSoftBody::Node*> parents;
+				parents.push_back(t.m_n[0]);
+				parents.push_back(t.m_n[1]);
+				parents.push_back(t.m_n[2]);
+				parents.push_back(t.m_n[3]);
+				optimal_parents = parents;
+				optimal_bary = bary;
+				min_bary_weight = new_min_bary_weight;
+				// stop searching if p is inside the tetrahedron at hand
+				if (bary[0] >= 0. && bary[1] >= 0. && bary[2] >= 0. && bary[3] >= 0.)
+				{
+					break;
+				}
+			}
+		}
+		psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
+		psb->m_renderNodesParents[i] = optimal_parents;
+	}
 }
 
-
 // Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense.
 void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb)
 {
-    psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
-    psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
-    psb->m_z.resize(psb->m_renderNodes.size());
-    for (int i = 0; i < psb->m_renderNodes.size(); ++i)
-    {
-        const btVector3& p = psb->m_renderNodes[i].m_x;
-        btVector4 bary;
-        btVector4 optimal_bary;
-        btScalar min_bary_weight = -SIMD_INFINITY;
-        btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
-        btScalar dist = 0, optimal_dist = 0;
-        for (int j = 0; j < psb->m_faces.size(); ++j)
-        {
-            const btSoftBody::Face& f = psb->m_faces[j];
-            btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x,  f.m_n[2]->m_x - f.m_n[0]->m_x);
-            btVector3 unit_n = n.normalized();
-            dist = (p-f.m_n[0]->m_x).dot(unit_n);
-            btVector3 proj_p = p - dist*unit_n;
-            getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary);
-            btScalar new_min_bary_weight = bary[0];
-            for (int k = 1; k < 3; ++k)
-            {
-                new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
-            }
-
-            // p is out of the current best triangle, we found a traingle that's better
-            bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight<0.);
-            // p is inside of the current best triangle, we found a triangle that's better
-            bool better_than_best_inside = (new_min_bary_weight>=0 &&  min_bary_weight>=0 && btFabs(dist)<btFabs(optimal_dist));
-
-            if (better_than_closest_outisde || better_than_best_inside)
-            {
-                btAlignedObjectArray<const btSoftBody::Node*> parents;
-                parents.push_back(f.m_n[0]);
-                parents.push_back(f.m_n[1]);
-                parents.push_back(f.m_n[2]);
-                optimal_parents = parents;
-                optimal_bary = bary;
-                optimal_dist = dist;
-                min_bary_weight = new_min_bary_weight;
-            }
-        }
-        psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
-        psb->m_renderNodesParents[i] = optimal_parents;
-        psb->m_z[i] = optimal_dist;
-    }
+	psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
+	psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
+	psb->m_z.resize(psb->m_renderNodes.size());
+	for (int i = 0; i < psb->m_renderNodes.size(); ++i)
+	{
+		const btVector3& p = psb->m_renderNodes[i].m_x;
+		btVector4 bary;
+		btVector4 optimal_bary;
+		btScalar min_bary_weight = -SIMD_INFINITY;
+		btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
+		btScalar dist = 0, optimal_dist = 0;
+		for (int j = 0; j < psb->m_faces.size(); ++j)
+		{
+			const btSoftBody::Face& f = psb->m_faces[j];
+			btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
+			btVector3 unit_n = n.normalized();
+			dist = (p - f.m_n[0]->m_x).dot(unit_n);
+			btVector3 proj_p = p - dist * unit_n;
+			getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary);
+			btScalar new_min_bary_weight = bary[0];
+			for (int k = 1; k < 3; ++k)
+			{
+				new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
+			}
+
+			// p is out of the current best triangle, we found a traingle that's better
+			bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight < 0.);
+			// p is inside of the current best triangle, we found a triangle that's better
+			bool better_than_best_inside = (new_min_bary_weight >= 0 && min_bary_weight >= 0 && btFabs(dist) < btFabs(optimal_dist));
+
+			if (better_than_closest_outisde || better_than_best_inside)
+			{
+				btAlignedObjectArray<const btSoftBody::Node*> parents;
+				parents.push_back(f.m_n[0]);
+				parents.push_back(f.m_n[1]);
+				parents.push_back(f.m_n[2]);
+				optimal_parents = parents;
+				optimal_bary = bary;
+				optimal_dist = dist;
+				min_bary_weight = new_min_bary_weight;
+			}
+		}
+		psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
+		psb->m_renderNodesParents[i] = optimal_parents;
+		psb->m_z[i] = optimal_dist;
+	}
 }

thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h

@@ -93,7 +93,7 @@ struct btSoftBodyHelpers
 								   int resy,
 								   int fixeds,
 								   bool gendiags,
-                                   btScalar perturbation = 0.);
+								   btScalar perturbation = 0.);
 	/* Create a patch with UV Texture Coordinates	*/
 	static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
 									 const btVector3& corner00,
@@ -142,21 +142,21 @@ struct btSoftBodyHelpers
 											bool bfacelinks,
 											bool btetralinks,
 											bool bfacesfromtetras);
-    static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
+	static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
 
-    static void writeObj(const char* file, const btSoftBody* psb);
-    
-    static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);
-    
-    static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary);
-    
-    static void interpolateBarycentricWeights(btSoftBody* psb);
-    
-    static void extrapolateBarycentricWeights(btSoftBody* psb);
-    
-    static void generateBoundaryFaces(btSoftBody* psb);
-    
-    static void duplicateFaces(const char* filename, const btSoftBody* psb);
+	static void writeObj(const char* file, const btSoftBody* psb);
+
+	static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);
+
+	static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary);
+
+	static void interpolateBarycentricWeights(btSoftBody* psb);
+
+	static void extrapolateBarycentricWeights(btSoftBody* psb);
+
+	static void generateBoundaryFaces(btSoftBody* psb);
+
+	static void duplicateFaces(const char* filename, const btSoftBody* psb);
 	/// Sort the list of links to move link calculations that are dependent upon earlier
 	/// ones as far as possible away from the calculation of those values
 	/// This tends to make adjacent loop iterations not dependent upon one another,

thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h

@@ -36,7 +36,7 @@ public:
 		CL_SIMD_SOLVER,
 		DX_SOLVER,
 		DX_SIMD_SOLVER,
-        DEFORMABLE_SOLVER
+		DEFORMABLE_SOLVER
 	};
 
 protected:

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

@@ -22,36 +22,36 @@ 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]) )
+#if !defined(get16bits)
+#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) {
-  unsigned int hash = len, tmp;
-  len>>=2;
-
-    /* Main loop */
-    for (;len > 0; len--) {
-        hash  += get16bits (data);
-        tmp    = (get16bits (data+2) << 11) ^ hash;
-        hash   = (hash << 16) ^ tmp;
-        data  += 2*sizeof (unsigned short);
-        hash  += hash >> 11;
-    }
+inline unsigned int HsiehHash(const char* data, int len)
+{
+	unsigned int hash = len, tmp;
+	len >>= 2;
 
+	/* Main loop */
+	for (; len > 0; len--)
+	{
+		hash += get16bits(data);
+		tmp = (get16bits(data + 2) << 11) ^ hash;
+		hash = (hash << 16) ^ tmp;
+		data += 2 * sizeof(unsigned short);
+		hash += hash >> 11;
+	}
 
-    /* Force "avalanching" of final 127 bits */
-    hash ^= hash << 3;
-    hash += hash >> 5;
-    hash ^= hash << 4;
-    hash += hash >> 17;
-    hash ^= hash << 25;
-    hash += hash >> 6;
+	/* Force "avalanching" of final 127 bits */
+	hash ^= hash << 3;
+	hash += hash >> 5;
+	hash ^= hash << 4;
+	hash += hash >> 17;
+	hash ^= hash << 25;
+	hash += hash >> 6;
 
-    return hash;
+	return hash;
 }
 
 template <const int CELLSIZE>
@@ -81,7 +81,7 @@ struct btSparseSdf
 
 	btAlignedObjectArray<Cell*> cells;
 	btScalar voxelsz;
-    btScalar m_defaultVoxelsz;
+	btScalar m_defaultVoxelsz;
 	int puid;
 	int ncells;
 	int m_clampCells;
@@ -103,16 +103,16 @@ struct btSparseSdf
 		//if this limit is reached, the SDF is reset (at the cost of some performance during the reset)
 		m_clampCells = clampCells;
 		cells.resize(hashsize, 0);
-        m_defaultVoxelsz = 0.25;
+		m_defaultVoxelsz = 0.25;
 		Reset();
 	}
 	//
-    
-    void setDefaultVoxelsz(btScalar sz)
-    {
-        m_defaultVoxelsz = sz;
-    }
-    
+
+	void setDefaultVoxelsz(btScalar sz)
+	{
+		m_defaultVoxelsz = sz;
+	}
+
 	void Reset()
 	{
 		for (int i = 0, ni = cells.size(); i < ni; ++i)
@@ -162,7 +162,7 @@ struct btSparseSdf
 		nqueries = 1;
 		nprobes = 1;
 		++puid;  ///@todo: Reset puid's when int range limit is reached	*/
-				 /* else setup a priority list...						*/
+		/* else setup a priority list...						*/
 	}
 	//
 	int RemoveReferences(btCollisionShape* pcs)
@@ -221,7 +221,7 @@ struct btSparseSdf
 			else
 			{
 				// printf("c->hash/c[0][1][2]=%d,%d,%d,%d\n", c->hash, c->c[0], c->c[1],c->c[2]);
-                        //printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b);
+				//printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b);
 
 				c = c->next;
 			}
@@ -363,7 +363,7 @@ struct btSparseSdf
 		myset.p = (void*)shape;
 		const char* ptr = (const char*)&myset;
 
-		unsigned int result = HsiehHash(ptr, sizeof(btS) );
+		unsigned int result = HsiehHash(ptr, sizeof(btS));
 
 		return result;
 	}

thirdparty/bullet/BulletSoftBody/poly34.cpp

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

thirdparty/bullet/BulletSoftBody/poly34.h

@@ -8,31 +8,31 @@
 // x - array of size 2
 // return 2: 2 real roots x[0], x[1]
 // return 0: pair of complex roots: x[0]i*x[1]
-int SolveP2(btScalar* x, btScalar a, btScalar b); // solve equation x^2 + a*x + b = 0
+int SolveP2(btScalar* x, btScalar a, btScalar b);  // solve equation x^2 + a*x + b = 0
 
 // x - array of size 3
 // return 3: 3 real roots x[0], x[1], x[2]
 // return 1: 1 real root x[0] and pair of complex roots: x[1]i*x[2]
-int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c); // solve cubic equation x^3 + a*x^2 + b*x + c = 0
+int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c);  // solve cubic equation x^3 + a*x^2 + b*x + c = 0
 
 // x - array of size 4
 // return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots
 // return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3],
 // return 0: two pair of complex roots: x[0]i*x[1],  x[2]i*x[3],
-int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d); // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method
+int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d);  // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method
 
 // x - array of size 5
 // return 5: 5 real roots x[0], x[1], x[2], x[3], x[4], possible multiple roots
 // return 3: 3 real roots x[0], x[1], x[2] and complex x[3]i*x[4],
 // return 1: 1 real root x[0] and two pair of complex roots: x[1]i*x[2],  x[3]i*x[4],
-int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
+int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e);  // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
 
 //-----------------------------------------------------------------------------
 // And some additional functions for internal use.
 // Your may remove this definitions from here
-int SolveP4Bi(btScalar* x, btScalar b, btScalar d); // solve equation x^4 + b*x^2 + d = 0
-int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d); // solve equation x^4 + b*x^2 + c*x + d = 0
-void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b); // returns as a+i*s,  sqrt(x+i*y)
-btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
-btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
+int SolveP4Bi(btScalar* x, btScalar b, btScalar d);                              // solve equation x^4 + b*x^2 + d = 0
+int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d);                  // solve equation x^4 + b*x^2 + c*x + d = 0
+void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b);                    // returns as a+i*s,  sqrt(x+i*y)
+btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d);     // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
+btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e);  // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
 #endif

thirdparty/bullet/LinearMath/btAlignedAllocator.cpp

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

thirdparty/bullet/LinearMath/btAlignedAllocator.h

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

thirdparty/bullet/LinearMath/btConvexHullComputer.cpp

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

thirdparty/bullet/LinearMath/btConvexHullComputer.h

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

thirdparty/bullet/LinearMath/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()