Update to bullet master (2.90)

modules/bullet/SCsub

@@ -175,6 +175,7 @@ if env["builtin_bullet"]:
         "BulletSoftBody/btDeformableContactProjection.cpp",
         "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp",
         "BulletSoftBody/btDeformableContactConstraint.cpp",
+        "BulletSoftBody/poly34.cpp",
         # clew
         "clew/clew.c",
         # LinearMath

platform/linuxbsd/detect.py

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

thirdparty/README.md

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

thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvt.h

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

thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

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

thirdparty/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp

@@ -468,7 +468,7 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall
 
 #ifdef RAYAABB2
 	btVector3 rayDir = (rayTarget - raySource);
-	rayDir.normalize();
+	rayDir.safeNormalize();// stephengold changed normalize to safeNormalize 2020-02-17
 	lambda_max = rayDir.dot(rayTarget - raySource);
 	///what about division by zero? --> just set rayDirection[i] to 1.0
 	btVector3 rayDirectionInverse;
@@ -554,7 +554,7 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback*
 
 #ifdef RAYAABB2
 	btVector3 rayDirection = (rayTarget - raySource);
-	rayDirection.normalize();
+	rayDirection.safeNormalize();// stephengold changed normalize to safeNormalize 2020-02-17
 	lambda_max = rayDirection.dot(rayTarget - raySource);
 	///what about division by zero? --> just set rayDirection[i] to 1.0
 	rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[0];

thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h

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

thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp

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

thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h

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

thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

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

thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -46,7 +46,7 @@ struct btContactSolverInfoData
 	btScalar m_sor;          //successive over-relaxation term
 	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_erp;          //error reduction for deformable constraints
 	btScalar m_globalCfm;    //constraint force mixing for contacts and non-contacts
 	btScalar m_frictionERP;  //error reduction for friction constraints
 	btScalar m_frictionCFM;  //constraint force mixing for friction constraints
@@ -67,6 +67,7 @@ struct btContactSolverInfoData
 	bool m_jointFeedbackInWorldSpace;
 	bool m_jointFeedbackInJointFrame;
 	int m_reportSolverAnalytics;
+	int m_numNonContactInnerIterations;
 };
 
 struct btContactSolverInfo : public btContactSolverInfoData
@@ -82,7 +83,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_numIterations = 10;
 		m_erp = btScalar(0.2);
 		m_erp2 = btScalar(0.2);
-        m_deformable_erp = btScalar(0.);
+		m_deformable_erp = btScalar(0.1);
 		m_globalCfm = btScalar(0.);
 		m_frictionERP = btScalar(0.2);  //positional friction 'anchors' are disabled by default
 		m_frictionCFM = btScalar(0.);
@@ -104,6 +105,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_jointFeedbackInWorldSpace = false;
 		m_jointFeedbackInJointFrame = false;
 		m_reportSolverAnalytics = 0;
+		m_numNonContactInnerIterations = 1;   // the number of inner iterations for solving motor constraint in a single iteration of the constraint solve
 	}
 };
 

thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp

@@ -876,7 +876,10 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		// will we not request a velocity with the wrong direction ?
 		// and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
 		// so the sign of the force that is really matters
-		info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
+		if (m_flags & BT_6DOF_FLAGS_USE_INFINITE_ERROR)
+			info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
+		else
+			info->m_constraintError[srow] = vel + f / m * (rotational ? -1 : 1);
 
 		btScalar minf = f < fd ? f : fd;
 		btScalar maxf = f < fd ? fd : f;

thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h

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

thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

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

thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

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

thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -136,8 +136,13 @@ void btRigidBody::setGravity(const btVector3& acceleration)
 
 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
 {
-	m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
-	m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+#ifdef BT_USE_OLD_DAMPING_METHOD
+	m_linearDamping = btMax(lin_damping, btScalar(0.0));
+	m_angularDamping = btMax(ang_damping, btScalar(0.0));
+#else
+	m_linearDamping = btClamped(lin_damping, btScalar(0.0), btScalar(1.0));
+	m_angularDamping = btClamped(ang_damping, btScalar(0.0), btScalar(1.0));
+#endif
 }
 
 ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
@@ -146,10 +151,9 @@ void btRigidBody::applyDamping(btScalar timeStep)
 	//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
 	//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
 
-//#define USE_OLD_DAMPING_METHOD 1
-#ifdef USE_OLD_DAMPING_METHOD
-	m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
-	m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+#ifdef BT_USE_OLD_DAMPING_METHOD
+	m_linearVelocity *= btMax((btScalar(1.0) - timeStep * m_linearDamping), btScalar(0.0));
+	m_angularVelocity *= btMax((btScalar(1.0) - timeStep * m_angularDamping), btScalar(0.0));
 #else
 	m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep);
 	m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep);
@@ -380,6 +384,9 @@ void btRigidBody::integrateVelocities(btScalar step)
 	{
 		m_angularVelocity *= (MAX_ANGVEL / step) / angvel;
 	}
+	#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+	clampVelocity(m_angularVelocity);
+	#endif
 }
 
 btQuaternion btRigidBody::getOrientation() const

thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h

@@ -305,6 +305,9 @@ public:
 	void applyTorque(const btVector3& torque)
 	{
 		m_totalTorque += torque * m_angularFactor;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_totalTorque);
+		#endif
 	}
 
 	void applyForce(const btVector3& force, const btVector3& rel_pos)
@@ -316,11 +319,17 @@ public:
 	void applyCentralImpulse(const btVector3& impulse)
 	{
 		m_linearVelocity += impulse * m_linearFactor * m_inverseMass;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_linearVelocity);
+		#endif
 	}
 
 	void applyTorqueImpulse(const btVector3& torque)
 	{
 		m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_angularVelocity);
+		#endif
 	}
 
 	void applyImpulse(const btVector3& impulse, const btVector3& rel_pos)
@@ -361,20 +370,46 @@ public:
     {
         m_pushVelocity = v;
     }
-    
+
+    #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+    void clampVelocity(btVector3& v) const {
+        v.setX(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getX()))
+        );
+        v.setY(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getY()))
+        );
+        v.setZ(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getZ()))
+        );
+    }
+    #endif
+
     void setTurnVelocity(const btVector3& v)
     {
         m_turnVelocity = v;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_turnVelocity);
+        #endif
     }
     
     void applyCentralPushImpulse(const btVector3& impulse)
     {
         m_pushVelocity += impulse * m_linearFactor * m_inverseMass;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_pushVelocity);
+        #endif
     }
     
     void applyTorqueTurnImpulse(const btVector3& torque)
     {
         m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_turnVelocity);
+        #endif
     }
 
 	void clearForces()
@@ -408,12 +443,18 @@ public:
 	{
 		m_updateRevision++;
 		m_linearVelocity = lin_vel;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_linearVelocity);
+		#endif
 	}
 
 	inline void setAngularVelocity(const btVector3& ang_vel)
 	{
 		m_updateRevision++;
 		m_angularVelocity = ang_vel;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_angularVelocity);
+		#endif
 	}
 
 	btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const

thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp

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

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -583,52 +583,6 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
 	}
 }
 
-btScalar btMultiBody::getKineticEnergy() const
-{
-	int num_links = getNumLinks();
-	// TODO: would be better not to allocate memory here
-	btAlignedObjectArray<btVector3> omega;
-	omega.resize(num_links + 1);
-	btAlignedObjectArray<btVector3> vel;
-	vel.resize(num_links + 1);
-	compTreeLinkVelocities(&omega[0], &vel[0]);
-
-	// we will do the factor of 0.5 at the end
-	btScalar result = m_baseMass * vel[0].dot(vel[0]);
-	result += omega[0].dot(m_baseInertia * omega[0]);
-
-	for (int i = 0; i < num_links; ++i)
-	{
-		result += m_links[i].m_mass * vel[i + 1].dot(vel[i + 1]);
-		result += omega[i + 1].dot(m_links[i].m_inertiaLocal * omega[i + 1]);
-	}
-
-	return 0.5f * result;
-}
-
-btVector3 btMultiBody::getAngularMomentum() const
-{
-	int num_links = getNumLinks();
-	// TODO: would be better not to allocate memory here
-	btAlignedObjectArray<btVector3> omega;
-	omega.resize(num_links + 1);
-	btAlignedObjectArray<btVector3> vel;
-	vel.resize(num_links + 1);
-	btAlignedObjectArray<btQuaternion> rot_from_world;
-	rot_from_world.resize(num_links + 1);
-	compTreeLinkVelocities(&omega[0], &vel[0]);
-
-	rot_from_world[0] = m_baseQuat;
-	btVector3 result = quatRotate(rot_from_world[0].inverse(), (m_baseInertia * omega[0]));
-
-	for (int i = 0; i < num_links; ++i)
-	{
-		rot_from_world[i + 1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent + 1];
-		result += (quatRotate(rot_from_world[i + 1].inverse(), (m_links[i].m_inertiaLocal * omega[i + 1])));
-	}
-
-	return result;
-}
 
 void btMultiBody::clearConstraintForces()
 {

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h

@@ -307,13 +307,6 @@ public:
 	//
 	btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const;
 
-	//
-	// calculate kinetic energy and angular momentum
-	// useful for debugging.
-	//
-
-	btScalar getKineticEnergy() const;
-	btVector3 getAngularMomentum() const;
 
 	//
 	// set external forces and torques. Note all external forces/torques are given in the WORLD frame.

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

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

thirdparty/bullet/BulletSoftBody/btConjugateResidual.h

@@ -0,0 +1,188 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_CONJUGATE_RESIDUAL_H
+#define BT_CONJUGATE_RESIDUAL_H
+#include <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 btConjugateResidual
+{
+    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;
+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;
+    }
+};
+#endif /* btConjugateResidual_h */
+

thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.cpp

@@ -23,12 +23,15 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned
 , m_backupVelocity(backup_v)
 , m_implicit(false)
 {
-    m_preconditioner = new MassPreconditioner(m_softBodies);
+    m_massPreconditioner = new MassPreconditioner(m_softBodies);
+    m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit);
+    m_preconditioner = m_KKTPreconditioner;
 }
 
 btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective()
 {
-    delete m_preconditioner;
+    delete m_KKTPreconditioner;
+    delete m_massPreconditioner;
 }
 
 void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt)
@@ -47,7 +50,7 @@ void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar
         m_lf[i]->reinitialize(nodeUpdated);
     }
     m_projection.reinitialize(nodeUpdated);
-    m_preconditioner->reinitialize(nodeUpdated);
+//    m_preconditioner->reinitialize(nodeUpdated);
 }
 
 void btDeformableBackwardEulerObjective::setDt(btScalar dt)
@@ -80,6 +83,33 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b)
              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)
@@ -134,7 +164,7 @@ void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &r
             m_lf[i]->addScaledDampingForce(dt, residual);
         }
     }
-    m_projection.project(residual);
+//    m_projection.project(residual);
 }
 
 btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const
@@ -186,9 +216,9 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack
 }
 
 //set constraints as projections
-void btDeformableBackwardEulerObjective::setConstraints()
+void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal)
 {
-    m_projection.setConstraints();
+    m_projection.setConstraints(infoGlobal);
 }
 
 void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r)

thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h

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

thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp

@@ -18,13 +18,15 @@
 #include "btDeformableBodySolver.h"
 #include "btSoftBodyInternals.h"
 #include "LinearMath/btQuickprof.h"
-static const int kMaxConjugateGradientIterations  = 50;
+static const int kMaxConjugateGradientIterations = 50;
 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_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
 }
@@ -41,7 +43,22 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
     {
         m_objective->computeResidual(solverdt, m_residual);
         m_objective->applyDynamicFriction(m_residual);
-        computeStep(m_dv, 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
@@ -63,7 +80,7 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
                     ++counter;
                 }
             }
-            
+
             m_objective->computeResidual(solverdt, m_residual);
             if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0)
             {
@@ -200,7 +217,10 @@ void btDeformableBodySolver::updateDv(btScalar scale)
 
 void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
 {
-    m_cg.solve(*m_objective, ddv, residual);
+    if (m_useProjection)
+        m_cg.solve(*m_objective, ddv, residual, false);
+    else
+        m_cr.solve(*m_objective, ddv, residual, false);
 }
 
 void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)
@@ -226,27 +246,22 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
     
     m_dt = dt;
     m_objective->reinitialize(nodeUpdated, dt);
+    updateSoftBodies();
 }
 
-void btDeformableBodySolver::setConstraints()
+void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
 {
     BT_PROFILE("setConstraint");
-    m_objective->setConstraints();
+    m_objective->setConstraints(infoGlobal);
 }
 
-btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies)
+btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 {
     BT_PROFILE("solveContactConstraints");
-    btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies);
+    btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies, infoGlobal);
     return maxSquaredResidual;
 }
 
-btScalar btDeformableBodySolver::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
-{
-    BT_PROFILE("solveSplitImpulse");
-    return m_objective->m_projection.solveSplitImpulse(infoGlobal);
-}
-
 void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
 {
      m_objective->m_projection.splitImpulseSetup(infoGlobal);
@@ -333,8 +348,10 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit)
                 m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
             }
             else
+            {
                 m_dv[counter] =  psb->m_nodes[j].m_v - m_backupVelocity[counter];
-            psb->m_nodes[j].m_v = m_backupVelocity[counter] + psb->m_nodes[j].m_vsplit;
+            }
+            psb->m_nodes[j].m_v = m_backupVelocity[counter];
             ++counter;
         }
     }
@@ -385,6 +402,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt)
 
 void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt)
 {
+    BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
     int i, ni;
     
     /* Update                */
@@ -423,40 +441,22 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
             n.m_v *= max_v;
         }
         n.m_q = n.m_x + n.m_v * dt;
+        n.m_penetration = 0;
     }
 
     /* Nodes                */
-    ATTRIBUTE_ALIGNED16(btDbvtVolume)
-    vol;
-    for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
-    {
-        btSoftBody::Node& n = psb->m_nodes[i];
-        btVector3 points[2] = {n.m_x, n.m_q};
-        vol = btDbvtVolume::FromPoints(points, 2);
-        vol.Expand(btVector3(psb->m_sst.radmrg, psb->m_sst.radmrg, psb->m_sst.radmrg));
-        psb->m_ndbvt.update(n.m_leaf, vol);
-    }
-
+    psb->updateNodeTree(true, true);
     if (!psb->m_fdbvt.empty())
     {
-        for (int i = 0; i < psb->m_faces.size(); ++i)
-        {
-            btSoftBody::Face& f = psb->m_faces[i];
-            btVector3 points[6] = {f.m_n[0]->m_x, f.m_n[0]->m_q,
-                                   f.m_n[1]->m_x, f.m_n[1]->m_q,
-                                   f.m_n[2]->m_x, f.m_n[2]->m_q};
-            vol = btDbvtVolume::FromPoints(points, 6);
-            vol.Expand(btVector3(psb->m_sst.radmrg, psb->m_sst.radmrg, psb->m_sst.radmrg));
-            psb->m_fdbvt.update(f.m_leaf, vol);
-        }
+        psb->updateFaceTree(true, true);
     }
-    /* Clear contacts        */
+    /* Clear contacts */
     psb->m_nodeRigidContacts.resize(0);
     psb->m_faceRigidContacts.resize(0);
     psb->m_faceNodeContacts.resize(0);
     /* Optimize dbvt's        */
-    psb->m_ndbvt.optimizeIncremental(1);
-    psb->m_fdbvt.optimizeIncremental(1);
+//    psb->m_ndbvt.optimizeIncremental(1);
+//    psb->m_fdbvt.optimizeIncremental(1);
 }
 
 

thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h

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

thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp

@@ -15,9 +15,9 @@
 
 #include "btDeformableContactConstraint.h"
 /* ================   Deformable Node Anchor   =================== */
-btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a)
+btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal)
 : m_anchor(&a)
-, btDeformableContactConstraint(a.m_cti.m_normal)
+, btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal)
 {
 }
 
@@ -79,14 +79,14 @@ btVector3 btDeformableNodeAnchorConstraint::getVa() const
     return va;
 }
 
-btScalar btDeformableNodeAnchorConstraint::solveConstraint()
+btScalar btDeformableNodeAnchorConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 {
     const btSoftBody::sCti& cti = m_anchor->m_cti;
     btVector3 va = getVa();
     btVector3 vb = getVb();
     btVector3 vr = (vb - va);
     // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0
-    const btScalar dn = btDot(vr, cti.m_normal);
+    const btScalar dn = btDot(vr, vr);
     // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
     btScalar residualSquare = dn*dn;
     btVector3 impulse = m_anchor->m_c0 * vr;
@@ -134,14 +134,15 @@ void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
 }
 
 /* ================   Deformable vs. Rigid   =================== */
-btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
+btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal)
 : m_contact(&c)
-, btDeformableContactConstraint(c.m_cti.m_normal)
+, btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal)
 {
     m_total_normal_dv.setZero();
     m_total_tangent_dv.setZero();
-    // penetration is non-positive. The magnitude of penetration is the depth of penetration.
-    m_penetration = btMin(btScalar(0), c.m_cti.m_offset);
+    // The magnitude of penetration is the depth of penetration.
+    m_penetration = c.m_cti.m_offset;
+//	m_penetration = btMin(btScalar(0),c.m_cti.m_offset);
 }
 
 btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
@@ -206,16 +207,16 @@ btVector3 btDeformableRigidContactConstraint::getVa() const
     return va;
 }
 
-btScalar btDeformableRigidContactConstraint::solveConstraint()
+btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 {
     const btSoftBody::sCti& cti = m_contact->m_cti;
     btVector3 va = getVa();
     btVector3 vb = getVb();
     btVector3 vr = vb - va;
-    const btScalar dn = btDot(vr, cti.m_normal);
+    btScalar dn = btDot(vr, cti.m_normal) + m_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;
+    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;
@@ -256,6 +257,8 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
     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)
     {
@@ -285,43 +288,17 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
             }
         }
     }
+//    va = getVa();
+//    vb = getVb();
+//    vr = vb - va;
+//    btScalar dn1 = btDot(vr, cti.m_normal) / 150;
+//    m_penetration += dn1;
     return residualSquare;
 }
-
-btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
-{
-    const btSoftBody::sCti& cti = m_contact->m_cti;
-    const btScalar dn = m_penetration;
-    if (dn != 0)
-    {
-        const btVector3 impulse = (m_contact->m_c0 * (cti.m_normal * dn / infoGlobal.m_timeStep));
-        // one iteration of the position impulse corrects all the position error at this timestep
-        m_penetration -= dn;
-        // apply impulse to deformable nodes involved and change their position
-        applySplitImpulse(impulse);
-        // apply impulse to the rigid/multibodies involved and change their position
-        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)
-        {
-            // todo xuchenhan@
-        }
-        return (m_penetration/infoGlobal.m_timeStep) * (m_penetration/infoGlobal.m_timeStep);
-    }
-    return 0;
-}
-
 /* ================   Node vs. Rigid   =================== */
-btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact)
+btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal)
     : m_node(contact.m_node)
-    , btDeformableRigidContactConstraint(contact)
+    , btDeformableRigidContactConstraint(contact, infoGlobal)
     {
     }
 
@@ -349,22 +326,17 @@ void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impul
     contact->m_node->m_v -= dv;
 }
 
-void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
-{
-    const btSoftBody::DeformableNodeRigidContact* contact = getContact();
-    btVector3 dv = impulse * contact->m_c2;
-    contact->m_node->m_vsplit -= dv;
-};
-
 /* ================   Face vs. Rigid   =================== */
-btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
+btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting)
 : m_face(contact.m_face)
-, btDeformableRigidContactConstraint(contact)
+, m_useStrainLimiting(useStrainLimiting)
+, btDeformableRigidContactConstraint(contact, infoGlobal)
 {
 }
 
 btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other)
 : m_face(other.m_face)
+, m_useStrainLimiting(other.m_useStrainLimiting)
 , btDeformableRigidContactConstraint(other)
 {
 }
@@ -411,47 +383,70 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
         v1 -= dv * contact->m_weights[1];
     if (im2 > 0)
         v2 -= dv * contact->m_weights[2];
-    
-    // apply strain limiting to prevent undamped modes
-    btScalar m01 = (btScalar(1)/(im0 + im1));
-    btScalar m02 = (btScalar(1)/(im0 + im2));
-    btScalar m12 = (btScalar(1)/(im1 + im2));
-    
-    btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
-    btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
-    btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
-    
-    v0 += dv0;
-    v1 += dv1;
-    v2 += dv2;
-}
-
-void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
-{
-    const btSoftBody::DeformableFaceRigidContact* contact = getContact();
-    btVector3 dv = impulse * contact->m_c2;
-    btSoftBody::Face* face = contact->m_face;
-
-    btVector3& v0 = face->m_n[0]->m_vsplit;
-    btVector3& v1 = face->m_n[1]->m_vsplit;
-    btVector3& v2 = face->m_n[2]->m_vsplit;
-    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
+		// apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%.
+		btScalar p = 0.01;
+		btVector3& x0 = face->m_n[0]->m_x;
+		btVector3& x1 = face->m_n[1]->m_x;
+		btVector3& x2 = face->m_n[2]->m_x;
+		const btVector3 x_diff[3] = {x1-x0, x2-x0, x2-x1};
+		const btVector3 v_diff[3] = {v1-v0, v2-v0, v2-v1};
+		btVector3 u[3];
+		btScalar x_diff_dot_u, dn[3];
+		btScalar dt = m_infoGlobal->m_timeStep;
+		for (int i = 0; i < 3; ++i)
+		{
+			btScalar x_diff_norm = x_diff[i].safeNorm();
+			btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm();
+			btScalar strainRate = x_diff_norm_new/x_diff_norm;
+			u[i] = v_diff[i];
+			u[i].safeNormalize();
+			if (x_diff_norm == 0 || (1-p <= strainRate && strainRate <= 1+p))
+			{
+				dn[i] = 0;
+				continue;
+			}
+			x_diff_dot_u = btDot(x_diff[i], u[i]);
+			btScalar s;
+			if (1-p > strainRate)
+			{
+				s = 1/dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p-2*p) * x_diff_norm * x_diff_norm));
+			}
+			else
+			{
+				s = 1/dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p+2*p) * x_diff_norm * x_diff_norm));
+			}
+			//		x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm();
+			//		strainRate = x_diff_norm_new/x_diff_norm;
+			dn[i] = s - v_diff[i].safeNorm();
+		}
+		btVector3 dv0 = im0 * (m01 * u[0]*(-dn[0]) + m02 * u[1]*-(dn[1]));
+		btVector3 dv1 = im1 * (m01 * u[0]*(dn[0]) + m12 * u[2]*(-dn[2]));
+		btVector3 dv2 = im2 * (m12 * u[2]*(dn[2]) + m02 * u[1]*(dn[1]));
+	#else
+		// apply strain limiting to prevent undamped modes
+		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;
+	}
 }
 
 /* ================   Face vs. Node   =================== */
-btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact)
+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)
+, btDeformableContactConstraint(contact.m_normal, infoGlobal)
 {
     m_total_normal_dv.setZero();
     m_total_tangent_dv.setZero();
@@ -487,7 +482,7 @@ btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n
     return dv * contact->m_weights[2];
 }
 
-btScalar btDeformableFaceNodeContactConstraint::solveConstraint()
+btScalar btDeformableFaceNodeContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 {
     btVector3 va = getVa();
     btVector3 vb = getVb();
@@ -577,15 +572,4 @@ void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impuls
     {
         v2 -= dvb * contact->m_weights[2];
     }
-    // todo: Face node constraints needs more work
-//    btScalar m01 = (btScalar(1)/(im0 + im1));
-//    btScalar m02 = (btScalar(1)/(im0 + im2));
-//    btScalar m12 = (btScalar(1)/(im1 + im2));
-//
-//    btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
-//    btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
-//    btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
-//    v0 += dv0;
-//    v1 += dv1;
-//    v2 += dv2;
 }

thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.h

@@ -24,34 +24,33 @@ public:
     // True if the friction is static
     // False if the friction is dynamic
     bool m_static;
-    
-    // normal of the contact
-    btVector3 m_normal;
-    
-    btDeformableContactConstraint(const btVector3& normal): m_static(false), m_normal(normal)
-    {
-    }
-    
-    btDeformableContactConstraint(bool isStatic, const btVector3& normal): m_static(isStatic), m_normal(normal)
-    {
-    }
-    
-    btDeformableContactConstraint(const btDeformableContactConstraint& other)
-    : m_static(other.m_static)
-    , m_normal(other.m_normal)
-    {
-        
-    }
-    btDeformableContactConstraint(){}
-    
+	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(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal)
+	{
+	}
+	
+	btDeformableContactConstraint(){}
+
+	btDeformableContactConstraint(const btDeformableContactConstraint& other)
+	: m_static(other.m_static)
+	, m_normal(other.m_normal)
+	, m_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() = 0;
-    
-    // solve the position error by applying an inelastic impulse that changes only the position (not velocity)
-    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) = 0;
+    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0;
     
     // get the velocity of the object A in the contact
     virtual btVector3 getVa() const = 0;
@@ -65,9 +64,6 @@ public:
     // apply impulse to the soft body node and/or face involved
     virtual void applyImpulse(const btVector3& impulse) = 0;
     
-    // apply position based impulse to the soft body node and/or face involved
-    virtual void applySplitImpulse(const btVector3& impulse) = 0;
-    
     // scale the penetration depth by erp
     virtual void setPenetrationScale(btScalar scale) = 0;
 };
@@ -77,29 +73,21 @@ public:
 class btDeformableStaticConstraint : public btDeformableContactConstraint
 {
 public:
-    const btSoftBody::Node* m_node;
+    btSoftBody::Node* m_node;
     
-    btDeformableStaticConstraint(){}
-    
-    btDeformableStaticConstraint(const btSoftBody::Node* node): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0))
+    btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal)
     {
     }
-    
+	btDeformableStaticConstraint(){}
     btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
     : m_node(other.m_node)
     , btDeformableContactConstraint(other)
     {
-        
     }
     
     virtual ~btDeformableStaticConstraint(){}
     
-    virtual btScalar solveConstraint()
-    {
-        return 0;
-    }
-    
-    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
+    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal)
     {
         return 0;
     }
@@ -120,7 +108,6 @@ public:
     }
     
     virtual void applyImpulse(const btVector3& impulse){}
-    virtual void applySplitImpulse(const btVector3& impulse){}
     virtual void setPenetrationScale(btScalar scale){}
 };
 
@@ -130,19 +117,15 @@ class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint
 {
 public:
     const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
-    
-    btDeformableNodeAnchorConstraint(){}
-    btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c);
+	
+    btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal);
     btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
+	btDeformableNodeAnchorConstraint(){}
     virtual ~btDeformableNodeAnchorConstraint()
     {
     }
-    virtual btScalar solveConstraint();
-    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
-    {
-        // todo xuchenhan@
-        return 0;
-    }
+    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
+
     // object A is the rigid/multi body, and object B is the deformable node/face
     virtual btVector3 getVa() const;
     // get the velocity of the deformable node in contact
@@ -152,10 +135,7 @@ public:
         return btVector3(0,0,0);
     }
     virtual void applyImpulse(const btVector3& impulse);
-    virtual void applySplitImpulse(const btVector3& impulse)
-    {
-        // todo xuchenhan@
-    };
+
     virtual void setPenetrationScale(btScalar scale){}
 };
 
@@ -169,10 +149,10 @@ public:
     btVector3 m_total_tangent_dv;
     btScalar m_penetration;
     const btSoftBody::DeformableRigidContact* m_contact;
-    
-    btDeformableRigidContactConstraint(){}
-    btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c);
+	
+    btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal);
     btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other);
+	btDeformableRigidContactConstraint(){}
     virtual ~btDeformableRigidContactConstraint()
     {
     }
@@ -180,9 +160,7 @@ public:
     // object A is the rigid/multi body, and object B is the deformable node/face
     virtual btVector3 getVa() const;
     
-    virtual btScalar solveConstraint();
-    
-    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
+    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
     
     virtual void setPenetrationScale(btScalar scale)
     {
@@ -196,12 +174,11 @@ class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactCo
 {
 public:
     // the deformable node in contact
-    const btSoftBody::Node* m_node;
-    
-    btDeformableNodeRigidContactConstraint(){}
-    btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact);
+    btSoftBody::Node* m_node;
+	
+    btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal);
     btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other);
-    
+	btDeformableNodeRigidContactConstraint(){}
     virtual ~btDeformableNodeRigidContactConstraint()
     {
     }
@@ -219,7 +196,6 @@ public:
     }
     
     virtual void applyImpulse(const btVector3& impulse);
-    virtual void applySplitImpulse(const btVector3& impulse);
 };
 
 //
@@ -228,10 +204,10 @@ class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactCo
 {
 public:
     const btSoftBody::Face* m_face;
-    btDeformableFaceRigidContactConstraint(){}
-    btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact);
+    bool m_useStrainLimiting;
+    btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting);
     btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
-    
+    btDeformableFaceRigidContactConstraint(): m_useStrainLimiting(false) {}
     virtual ~btDeformableFaceRigidContactConstraint()
     {
     }
@@ -249,7 +225,6 @@ public:
     }
     
     virtual void applyImpulse(const btVector3& impulse);
-    virtual void applySplitImpulse(const btVector3& impulse);
 };
 
 //
@@ -263,19 +238,11 @@ public:
     btVector3 m_total_normal_dv;
     btVector3 m_total_tangent_dv;
     
-    btDeformableFaceNodeContactConstraint(){}
-    
-    btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact);
-    
+    btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal);
+	btDeformableFaceNodeContactConstraint(){}
     virtual ~btDeformableFaceNodeContactConstraint(){}
     
-    virtual btScalar solveConstraint();
-    
-    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
-    {
-        // todo: xuchenhan@
-        return 0;
-    }
+    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
     
     // get the velocity of the object A in the contact
     virtual btVector3 getVa() const;
@@ -293,10 +260,7 @@ public:
     }
     
     virtual void applyImpulse(const btVector3& impulse);
-    virtual void applySplitImpulse(const btVector3& impulse)
-    {
-        // todo xuchenhan@
-    }
+
     virtual void setPenetrationScale(btScalar scale){}
 };
 #endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */

thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp

@@ -17,7 +17,7 @@
 #include "btDeformableMultiBodyDynamicsWorld.h"
 #include <algorithm>
 #include <cmath>
-btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies)
+btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 {
 	btScalar residualSquare = 0;
 	for (int i = 0; i < numDeformableBodies; ++i)
@@ -32,25 +32,25 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod
 			for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
 			{
 				btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 			for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
 			{
 				btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 			for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
 			{
 				btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 			for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
 			{
 				btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 		}
@@ -77,39 +77,8 @@ void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo&
 	}
 }
 
-btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
-{
-	btScalar residualSquare = 0;
-	for (int i = 0; i < m_softBodies.size(); ++i)
-	{
-		// node constraints
-		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
-		{
-			btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
-			btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
-			residualSquare = btMax(residualSquare, localResidualSquare);
-		}
-		// anchor constraints
-		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
-		{
-			btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[i][j];
-			btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
-			residualSquare = btMax(residualSquare, localResidualSquare);
-		}
-		// face constraints
-		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
-		{
-			btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
-			btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
-			residualSquare = btMax(residualSquare, localResidualSquare);
-		}
-
-	}
-	return residualSquare;
-}
-
-void btDeformableContactProjection::setConstraints()
-{
+void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
+{  
 	BT_PROFILE("setConstraints");
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
@@ -124,7 +93,7 @@ void btDeformableContactProjection::setConstraints()
 		{
 			if (psb->m_nodes[j].m_im == 0)
 			{
-				btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]);
+				btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal);
 				m_staticConstraints[i].push_back(static_constraint);
 			}
 		}
@@ -139,7 +108,7 @@ void btDeformableContactProjection::setConstraints()
 				continue;
 			}
 			anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
-			btDeformableNodeAnchorConstraint constraint(anchor);
+			btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal);
 			m_nodeAnchorConstraints[i].push_back(constraint);
 		}
 		
@@ -152,7 +121,7 @@ void btDeformableContactProjection::setConstraints()
 			{
 				continue;
 			}
-			btDeformableNodeRigidContactConstraint constraint(contact);
+			btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
 			btVector3 va = constraint.getVa();
 			btVector3 vb = constraint.getVb();
 			const btVector3 vr = vb - va;
@@ -173,7 +142,7 @@ void btDeformableContactProjection::setConstraints()
 			{
 				continue;
 			}
-			btDeformableFaceRigidContactConstraint constraint(contact);
+			btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
 			btVector3 va = constraint.getVa();
 			btVector3 vb = constraint.getVb();
 			const btVector3 vr = vb - va;
@@ -184,253 +153,404 @@ void btDeformableContactProjection::setConstraints()
 				m_faceRigidConstraints[i].push_back(constraint);
 			}
 		}
-		
-		// set Deformable Face vs. Deformable Node constraint
-		for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j)
-		{
-			const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j];
-
-			btDeformableFaceNodeContactConstraint constraint(contact);
-			btVector3 va = constraint.getVa();
-			btVector3 vb = constraint.getVb();
-			const btVector3 vr = vb - va;
-			const btScalar dn = btDot(vr, contact.m_normal);
-			if (dn > -SIMD_EPSILON)
-			{
-				m_deformableConstraints[i].push_back(constraint);
-			}
-		}
 	}
 }
 
 void btDeformableContactProjection::project(TVStack& x)
 {
-	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;
-		}
-	}
+#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;
+        }
+    }
+#else
+    btReducedVector p(x.size());
+    for (int i = 0; i < m_projections.size(); ++i)
+    {
+        p += (m_projections[i].dot(x) * m_projections[i]);
+    }
+    for (int i = 0; i < p.m_indices.size(); ++i)
+    {
+        x[p.m_indices[i]] -= p.m_vecs[i];
+    }
+#endif
 }
 
 void btDeformableContactProjection::setProjection()
 {
-	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;
-			if (m_projectionsDict.find(index) == NULL)
+#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);
+                    }
+                }
+            }
+        }
+    }
+#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;
+			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);
+            }
+			if (m_faceRigidConstraints[i][j].m_static)
 			{
-				m_projectionsDict.insert(index, units);
-			}
-			else
-			{
-				btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-				for (int k = 0; k < 3; ++k)
+				for (int l = 0; l < 3; ++l)
 				{
-					projections.push_back(units[k]);
-				}
-			}
-		}
-		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
-		{
-			int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
-			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;
-			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];
+					
+					btReducedVector rv(dof);
 					for (int k = 0; k < 3; ++k)
 					{
-						projections.push_back(units[k]);
+						rv.m_indices.push_back(face->m_n[k]->index);
+						btVector3 v(0,0,0);
+						v[l] = bary[k];
+						rv.m_vecs.push_back(v);
+                        rv.sort();
 					}
+					m_projections.push_back(rv);
 				}
 			}
 			else
 			{
-				if (m_projectionsDict.find(index) == NULL)
+				btReducedVector rv(dof);
+				for (int k = 0; k < 3; ++k)
 				{
-					btAlignedObjectArray<btVector3> projections;
-					projections.push_back(m_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);
+					rv.m_indices.push_back(face->m_n[k]->index);
+					rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal);
+                    rv.sort();
 				}
+				m_projections.push_back(rv);
 			}
 		}
-		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
-		{
-			const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
-			for (int k = 0; k < 3; ++k)
-			{
-				const 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 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);
-					}
-				}
-			}
-		}
-		for (int j = 0; j < m_deformableConstraints[i].size(); ++j)
-		{
-			const btSoftBody::Face* face = m_deformableConstraints[i][j].m_face;
-			for (int k = 0; k < 3; ++k)
-			{
-				const btSoftBody::Node* node = face->m_n[k];
-				int index = node->index;
-				if (m_deformableConstraints[i][j].m_static)
-				{
-					if (m_projectionsDict.find(index) == NULL)
-					{
-						m_projectionsDict.insert(index, units);
-					}
-					else
-					{
-						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-						for (int k = 0; k < 3; ++k)
-						{
-							projections.push_back(units[k]);
-						}
-					}
-				}
-				else
-				{
-					if (m_projectionsDict.find(index) == NULL)
-					{
-						btAlignedObjectArray<btVector3> projections;
-						projections.push_back(m_deformableConstraints[i][j].m_normal);
-						m_projectionsDict.insert(index, projections);
-					}
-					else
-					{
-						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-						projections.push_back(m_deformableConstraints[i][j].m_normal);
-					}
-				}
-			}
+    }
+    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]);
+    }
+}
+
+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;
 			
-			const btSoftBody::Node* node = m_deformableConstraints[i][j].m_node;
-			int index = node->index;
-			if (m_deformableConstraints[i][j].m_static)
+            btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
+            btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 3;
+			for (int k = 0; k<3; ++k)
 			{
-				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]);
-					}
-				}
+				face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
+				lm.m_indices[k] = face->m_n[k]->index;
+				lm.m_weights[k] = bary[k];
+			}
+            if (m_faceRigidConstraints[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
 			{
-				if (m_projectionsDict.find(index) == NULL)
-				{
-					btAlignedObjectArray<btVector3> projections;
-					projections.push_back(m_deformableConstraints[i][j].m_normal);
-					m_projectionsDict.insert(index, projections);
-				}
-				else
-				{
-					btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-					projections.push_back(m_deformableConstraints[i][j].m_normal);
-				}
+				lm.m_num_constraints = 1;
+				lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
 			}
+            m_lagrangeMultipliers.push_back(lm);
 		}
 	}
 }
 
-
+//
 void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 {
 	for (int i = 0; i < m_softBodies.size(); ++i)
@@ -502,7 +622,12 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 		m_faceRigidConstraints[i].clear();
 		m_deformableConstraints[i].clear();
 	}
-	m_projectionsDict.clear();
+#ifndef USE_MGS
+    m_projectionsDict.clear();
+#else
+    m_projections.clear();
+#endif
+    m_lagrangeMultipliers.clear();
 }
 
 

thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h

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

thirdparty/bullet/BulletSoftBody/btDeformableCorotatedForce.h

@@ -114,6 +114,8 @@ public:
     {
     }
     
+    virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){}
+    
     virtual btDeformableLagrangianForceType getForceType()
     {
         return BT_COROTATED_FORCE;

thirdparty/bullet/BulletSoftBody/btDeformableGravityForce.h

@@ -50,6 +50,8 @@ public:
     {
     }
     
+    virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){}
+    
     virtual void addScaledGravityForce(btScalar scale, TVStack& force)
     {
         int numNodes = getNumNodes();

thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h

@@ -26,7 +26,8 @@ enum btDeformableLagrangianForceType
     BT_MASSSPRING_FORCE = 2,
     BT_COROTATED_FORCE = 3,
     BT_NEOHOOKEAN_FORCE = 4,
-    BT_LINEAR_ELASTICITY_FORCE = 5
+    BT_LINEAR_ELASTICITY_FORCE = 5,
+    BT_MOUSE_PICKING_FORCE = 6
 };
 
 static inline double randomDouble(double low, double high)
@@ -53,6 +54,9 @@ public:
     // add damping df
     virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
     
+    // build diagonal of A matrix
+    virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0;
+    
     // add elastic df
     virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
     
@@ -85,6 +89,11 @@ public:
         m_softBodies.push_back(psb);
     }
     
+    virtual void removeSoftBody(btSoftBody* psb)
+    {
+        m_softBodies.remove(psb);
+    }
+    
     virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
     {
         m_nodes = nodes;

thirdparty/bullet/BulletSoftBody/btDeformableMassSpringForce.h

@@ -149,6 +149,52 @@ public:
         }
     }
     
+    virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA)
+    {
+        // implicit damping force differential
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
+            btScalar scaled_k_damp = m_dampingStiffness * scale;
+            for (int j = 0; j < psb->m_links.size(); ++j)
+            {
+                const btSoftBody::Link& link = psb->m_links[j];
+                btSoftBody::Node* node1 = link.m_n[0];
+                btSoftBody::Node* node2 = link.m_n[1];
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                if (m_momentum_conserving)
+                {
+                    if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
+                    {
+                        btVector3 dir = (node2->m_x - node1->m_x).normalized();
+                        for (int d = 0; d < 3; ++d)
+                        {
+                            if (node1->m_im > 0)
+                                diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d];
+                            if (node2->m_im > 0)
+                                diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d];
+                        }
+                    }
+                }
+                else
+                {
+                    for (int d = 0; d < 3; ++d)
+                    {
+                        if (node1->m_im > 0)
+                            diagA[id1][d] -= scaled_k_damp;
+                        if (node2->m_im > 0)
+                            diagA[id2][d] -= scaled_k_damp;
+                    }
+                }
+            }
+        }
+    }
+    
     virtual double totalElasticEnergy(btScalar dt)
     {
         double energy = 0;

thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h

@@ -0,0 +1,145 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_MOUSE_PICKING_FORCE_H
+#define BT_MOUSE_PICKING_FORCE_H
+
+#include "btDeformableLagrangianForce.h"
+
+class btDeformableMousePickingForce : public btDeformableLagrangianForce
+{
+    // If true, the damping force will be in the direction of the spring
+    // If false, the damping force will be in the direction of the velocity
+    btScalar m_elasticStiffness, m_dampingStiffness;
+    const btSoftBody::Face& m_face;
+    btVector3 m_mouse_pos;
+    btScalar m_maxForce;
+public:
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, 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;
+    }
+    
+};
+
+#endif /* btMassSpring_h */

thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -32,7 +32,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
             m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
             // solver body velocity -> rigid body velocity
             solverBodyWriteBack(infoGlobal);
-            btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies);
+            btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies, infoGlobal);
             // update rigid body velocity in rigid/deformable contact
             m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
             // solver body velocity <- rigid body velocity
@@ -112,7 +112,7 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI
     if (infoGlobal.m_splitImpulse)
     {
         {
-            m_deformableSolver->splitImpulseSetup(infoGlobal);
+//            m_deformableSolver->splitImpulseSetup(infoGlobal);
             for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
             {
                 btScalar leastSquaresResidual = 0.f;
@@ -127,8 +127,8 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI
                         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);
+//                    btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
+//                    leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
                 }
                 if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
                 {

thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -22,7 +22,6 @@ Call internalStepSimulation multiple times, to achieve 240Hz (4 steps of 60Hz).
 2. Detect discrete collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*.
 
 3a. Solve all constraints, including LCP. Contact, position correction due to numerical drift, friction, and anchors for deformable.
-    TODO: add option for positional drift correction (using vel_target += erp * pos_error/dt
 
 3b. 5 Newton steps (multiple step). Conjugent Gradient solves linear system. Deformable Damping: Then velocities of deformable bodies v_{n+1} are solved in
         M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass,
@@ -58,14 +57,20 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
 	m_sbi.water_density = 0;
 	m_sbi.water_offset = 0;
 	m_sbi.water_normal = btVector3(0, 0, 0);
-	m_sbi.m_gravity.setValue(0, -10, 0);
+	m_sbi.m_gravity.setValue(0, -9.8, 0);
 	m_internalTime = 0.0;
 	m_implicit = false;
 	m_lineSearch = false;
-	m_selfCollision = true;
+    m_useProjection = true;
+	m_ccdIterations = 5;
 	m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
 }
 
+btDeformableMultiBodyDynamicsWorld::~btDeformableMultiBodyDynamicsWorld()
+{
+    delete m_solverDeformableBodyIslandCallback;
+}
+
 void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
     BT_PROFILE("internalSingleStepSimulation");
@@ -74,20 +79,16 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
         (*m_internalPreTickCallback)(this, timeStep);
     }
     reinitialize(timeStep);
+    
     // add gravity to velocity of rigid and multi bodys
     applyRigidBodyGravity(timeStep);
     
     ///apply gravity and explicit force to velocity, predict motion
     predictUnconstraintMotion(timeStep);
     
-    ///perform collision detection
+    ///perform collision detection that involves rigid/multi bodies
     btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
     
-    if (m_selfCollision)
-    {
-        softBodySelfCollision();
-    }
-    
     btMultiBodyDynamicsWorld::calculateSimulationIslands();
     
     beforeSolverCallbacks(timeStep);
@@ -96,7 +97,13 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
     solveConstraints(timeStep);
     
     afterSolverCallbacks(timeStep);
-    
+
+	performDeformableCollisionDetection();
+
+    applyRepulsionForce(timeStep);
+
+    performGeometricCollisions(timeStep);
+
     integrateTransforms(timeStep);
     
     ///update vehicle simulation
@@ -107,6 +114,27 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
     // ///////////////////////////////
 }
 
+void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection()
+{
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		m_softBodies[i]->m_softSoftCollision = true;
+	}
+	
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		for (int j = i; j < m_softBodies.size(); ++j)
+		{
+			m_softBodies[i]->defaultCollisionHandler(m_softBodies[j]);
+		}
+	}
+	
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		m_softBodies[i]->m_softSoftCollision = false;
+	}
+}
+
 void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
 {
     for (int i = 0; i < m_softBodies.size(); i++)
@@ -131,10 +159,106 @@ void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep
     btMultiBodyDynamicsWorld::updateActivationState(timeStep);
 }
 
+void btDeformableMultiBodyDynamicsWorld::applyRepulsionForce(btScalar timeStep)
+{
+    BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce");
+    for (int i = 0; i < m_softBodies.size(); i++)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        if (psb->isActive())
+        {
+			psb->applyRepulsionForce(timeStep, true);
+        }
+    }
+}
+
+void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar timeStep)
+{
+	BT_PROFILE("btDeformableMultiBodyDynamicsWorld::performGeometricCollisions");
+    // refit the BVH tree for CCD
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (psb->isActive())
+		{
+			m_softBodies[i]->updateFaceTree(true, false);
+			m_softBodies[i]->updateNodeTree(true, false);
+			for (int j = 0; j < m_softBodies[i]->m_faces.size(); ++j)
+			{
+				btSoftBody::Face& f = m_softBodies[i]->m_faces[j];
+				f.m_n0 = (f.m_n[1]->m_x - f.m_n[0]->m_x).cross(f.m_n[2]->m_x - f.m_n[0]->m_x);
+			}
+		}
+	}
+
+	// clear contact points & update DBVT
+	for (int r = 0; r < m_ccdIterations; ++r)
+	{
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				// clear contact points in the previous iteration
+				psb->m_faceNodeContacts.clear();
+
+				// update m_q and normals for CCD calculation
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + timeStep * psb->m_nodes[j].m_v;
+				}
+				for (int j = 0; j < psb->m_faces.size(); ++j)
+				{
+					btSoftBody::Face& f = psb->m_faces[j];
+					f.m_n1 = (f.m_n[1]->m_q - f.m_n[0]->m_q).cross(f.m_n[2]->m_q - f.m_n[0]->m_q);
+					f.m_vn = (f.m_n[1]->m_v - f.m_n[0]->m_v).cross(f.m_n[2]->m_v - f.m_n[0]->m_v) * timeStep * timeStep;
+				}
+			}
+        }
+
+		// apply CCD to register new contact points
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			for (int j = i; j < m_softBodies.size(); ++j)
+			{
+				btSoftBody* psb1 = m_softBodies[i];
+				btSoftBody* psb2 = m_softBodies[j];
+				if (psb1->isActive() && psb2->isActive())
+				{
+					m_softBodies[i]->geometricCollisionHandler(m_softBodies[j]);
+				}
+			}
+        }
+
+		int penetration_count = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				penetration_count += psb->m_faceNodeContacts.size();
+			}
+		}
+		if (penetration_count == 0)
+		{
+			break;
+		}
+
+		// apply inelastic impulse
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				psb->applyRepulsionForce(timeStep, false);
+			}
+		}
+	}
+}
 
 void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
 {
-    m_deformableBodySolver->updateSoftBodies();
+    BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision");
     for (int i = 0; i < m_softBodies.size(); i++)
     {
         btSoftBody* psb = m_softBodies[i];
@@ -192,8 +316,6 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
                 }
             }
             node.m_x  =  node.m_x + timeStep * node.m_v;
-            node.m_v -= node.m_vsplit;
-            node.m_vsplit.setZero();
             node.m_q = node.m_x;
             node.m_vn = node.m_v;
         }
@@ -255,6 +377,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 
 void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 {
+    BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints");
     // save v_{n+1}^* velocity after explicit forces
     m_deformableBodySolver->backupVelocity();
     
@@ -265,8 +388,11 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
     solveContactConstraints();
     
     // set up the directions in which the velocity does not change in the momentum solve
-    m_deformableBodySolver->m_objective->m_projection.setProjection();
-    
+    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
@@ -280,7 +406,7 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 void btDeformableMultiBodyDynamicsWorld::setupConstraints()
 {
     // set up constraints between multibody and deformable bodies
-    m_deformableBodySolver->setConstraints();
+    m_deformableBodySolver->setConstraints(m_solverInfo);
     
     // set up constraints among multibodies
     {
@@ -403,6 +529,17 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
     dispatchInfo.m_stepCount = 0;
     dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
     btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
+    if (m_useProjection)
+    {
+        m_deformableBodySolver->m_useProjection = true;
+//        m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true;
+        m_deformableBodySolver->m_objective->m_preconditioner =  m_deformableBodySolver->m_objective->m_massPreconditioner;
+    }
+    else
+    {
+        m_deformableBodySolver->m_objective->m_preconditioner =  m_deformableBodySolver->m_objective->m_KKTPreconditioner;
+    }
+        
 }
 
 
@@ -566,6 +703,24 @@ void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableL
     }
 }
 
+void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force)
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+    int removed_index = -1;
+    for (int i = 0; i < forces.size(); ++i)
+    {
+        if (forces[i]->getForceType() == force->getForceType())
+        {
+            forces[i]->removeSoftBody(psb);
+            if (forces[i]->m_softBodies.size() == 0)
+                removed_index = i;
+            break;
+        }
+    }
+    if (removed_index >= 0)
+        forces.removeAtIndex(removed_index);
+}
+
 void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
 {
     m_softBodies.remove(body);

thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

@@ -46,10 +46,10 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
     bool m_drawClusterTree;
     btSoftBodyWorldInfo m_sbi;
     btScalar m_internalTime;
-    int m_contact_iterations;
+    int m_ccdIterations;
     bool m_implicit;
     bool m_lineSearch;
-    bool m_selfCollision;
+    bool m_useProjection;
     DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback;
     
     typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
@@ -80,9 +80,7 @@ public:
         m_solverCallback = cb;
     }
     
-    virtual ~btDeformableMultiBodyDynamicsWorld()
-    {
-    }
+    virtual ~btDeformableMultiBodyDynamicsWorld();
     
     virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
     {
@@ -133,6 +131,8 @@ public:
     
     void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
     
+    void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
+    
     void removeSoftBody(btSoftBody* body);
     
     void removeCollisionObject(btCollisionObject* collisionObject);
@@ -142,6 +142,8 @@ public:
     
     void setupConstraints();
     
+    void performDeformableCollisionDetection();
+    
     void solveMultiBodyConstraints();
     
     void solveContactConstraints();
@@ -159,7 +161,151 @@ public:
     {
         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;
+#if 0
+#ifdef RECALCULATE_AABB
+                btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+                collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+#else
+                //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
+                const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
+                const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
+#endif
+#endif
+                //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
+                //culling already done by broadphase
+                //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
+                {
+                    m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
+                                           collisionObject,
+                                           collisionObject->getCollisionShape(),
+                                           collisionObject->getWorldTransform(),
+                                           m_resultCallback);
+                }
+            }
+            return true;
+        }
+    };
 
+    
+    
+    void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
+    {
+        BT_PROFILE("rayTest");
+        /// use the broadphase to accelerate the search for objects, based on their aabb
+        /// and for each object with ray-aabb overlap, perform an exact ray test
+        btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
+        
+#ifndef USE_BRUTEFORCE_RAYBROADPHASE
+        m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
+#else
+        for (int i = 0; i < this->getNumCollisionObjects(); i++)
+        {
+            rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
+        }
+#endif  //USE_BRUTEFORCE_RAYBROADPHASE
+    }
+    
+    void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
+                                                     btCollisionObject* collisionObject,
+                                                     const btCollisionShape* collisionShape,
+                                                     const btTransform& colObjWorldTransform,
+                                                     RayResultCallback& resultCallback) const
+    {
+        if (collisionShape->isSoftBody())
+        {
+            btSoftBody* softBody = btSoftBody::upcast(collisionObject);
+            if (softBody)
+            {
+                btSoftBody::sRayCast softResult;
+                if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
+                {
+                    if (softResult.fraction <= resultCallback.m_closestHitFraction)
+                    {
+                        btCollisionWorld::LocalShapeInfo shapeInfo;
+                        shapeInfo.m_shapePart = 0;
+                        shapeInfo.m_triangleIndex = softResult.index;
+                        // get the normal
+                        btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
+                        btVector3 normal = -rayDir;
+                        normal.normalize();
+                        {
+                            normal = softBody->m_faces[softResult.index].m_normal;
+                            if (normal.dot(rayDir) > 0)
+                            {
+                                // normal always point toward origin of the ray
+                                normal = -normal;
+                            }
+                        }
+                        
+                        btCollisionWorld::LocalRayResult rayResult(collisionObject,
+                                                                   &shapeInfo,
+                                                                   normal,
+                                                                   softResult.fraction);
+                        bool normalInWorldSpace = true;
+                        resultCallback.addSingleResult(rayResult, normalInWorldSpace);
+                    }
+                }
+            }
+        }
+        else
+        {
+            btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
+        }
+    }
 };
 
 #endif  //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H

thirdparty/bullet/BulletSoftBody/btDeformableNeoHookeanForce.h

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

thirdparty/bullet/BulletSoftBody/btPreconditioner.h

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

thirdparty/bullet/BulletSoftBody/btSoftBody.cpp

@@ -18,12 +18,114 @@ subject to the following restrictions:
 #include "BulletSoftBody/btSoftBodySolvers.h"
 #include "btSoftBodyData.h"
 #include "LinearMath/btSerializer.h"
+#include "LinearMath/btImplicitQRSVD.h"
 #include "LinearMath/btAlignedAllocator.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
 #include <iostream>
+//
+static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& leafNodes, btAlignedObjectArray<btAlignedObjectArray<int> >& adj)
+{
+	int N = leafNodes.size();
+	if (N == 0)
+	{
+		return NULL;
+	}
+	while (N > 1)
+	{
+		btAlignedObjectArray<bool> marked;
+		btAlignedObjectArray<btDbvtNode*> newLeafNodes;
+		btAlignedObjectArray<std::pair<int,int> > childIds;
+		btAlignedObjectArray<btAlignedObjectArray<int> > newAdj;
+		marked.resize(N);
+		for (int i = 0; i < N; ++i)
+			marked[i] = false;
+		
+		// pair adjacent nodes into new(parent) node
+		for (int i = 0; i < N; ++i)
+		{
+			if (marked[i])
+				continue;
+			bool merged = false;
+			for (int j = 0; j < adj[i].size(); ++j)
+			{
+				int n = adj[i][j];
+				if (!marked[adj[i][j]])
+				{
+					btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
+					node->parent = NULL;
+					node->childs[0] = leafNodes[i];
+					node->childs[1] = leafNodes[n];
+					leafNodes[i]->parent = node;
+					leafNodes[n]->parent = node;
+					newLeafNodes.push_back(node);
+					childIds.push_back(std::make_pair(i,n));
+					merged = true;
+					marked[n] = true;
+					break;
+				}
+			}
+			if (!merged)
+			{
+				newLeafNodes.push_back(leafNodes[i]);
+				childIds.push_back(std::make_pair(i,-1));
+			}
+			marked[i] = true;
+		}
+		// update adjacency matrix
+		newAdj.resize(newLeafNodes.size());
+		for (int i = 0; i < newLeafNodes.size(); ++i)
+		{
+			for (int j = i+1; j < newLeafNodes.size(); ++j)
+			{
+				bool neighbor = false;
+				const btAlignedObjectArray<int>& leftChildNeighbors = adj[childIds[i].first];
+				for (int k = 0; k < leftChildNeighbors.size(); ++k)
+				{
+					if (leftChildNeighbors[k] == childIds[j].first || leftChildNeighbors[k] == childIds[j].second)
+					{
+						neighbor = true;
+						break;
+					}
+				}
+				if (!neighbor && childIds[i].second != -1)
+				{
+					const btAlignedObjectArray<int>& rightChildNeighbors = adj[childIds[i].second];
+					for (int k = 0; k < rightChildNeighbors.size(); ++k)
+					{
+						if (rightChildNeighbors[k] == childIds[j].first || rightChildNeighbors[k] == childIds[j].second)
+						{
+							neighbor = true;
+							break;
+						}
+					}
+				}
+				if (neighbor)
+				{
+					newAdj[i].push_back(j);
+					newAdj[j].push_back(i);
+				}
+			}
+		}
+		leafNodes = newLeafNodes;
+		//this assignment leaks memory, the assignment doesn't do a deep copy, for now a manual copy
+		//adj = newAdj;
+		adj.clear();
+		adj.resize(newAdj.size());
+		for (int i = 0; i < newAdj.size(); i++)
+		{
+			for (int j = 0; j < newAdj[i].size(); j++)
+			{
+				adj[i].push_back(newAdj[i][j]);
+			}
+		}
+		N = leafNodes.size();
+	}
+	return leafNodes[0];
+}
+
 //
 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
 	: m_softBodySolver(0), m_worldInfo(worldInfo)
@@ -41,6 +143,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV
 	/* Nodes			*/
 	const btScalar margin = getCollisionShape()->getMargin();
 	m_nodes.resize(node_count);
+    m_X.resize(node_count);
 	for (int i = 0, ni = node_count; i < ni; ++i)
 	{
 		Node& n = m_nodes[i];
@@ -51,8 +154,11 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV
 		n.m_im = n.m_im > 0 ? 1 / n.m_im : 0;
 		n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n);
 		n.m_material = pm;
+        m_X[i] = n.m_x;
 	}
 	updateBounds();
+	setCollisionQuadrature(3);
+	m_fdbvnt = 0;
 }
 
 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo)
@@ -111,15 +217,18 @@ void btSoftBody::initDefaults()
 	m_collisionShape = new btSoftBodyCollisionShape(this);
 	m_collisionShape->setMargin(0.25f);
 
-	m_initialWorldTransform.setIdentity();
+	m_worldTransform.setIdentity();
 
 	m_windVelocity = btVector3(0, 0, 0);
 	m_restLengthScale = btScalar(1.0);
-    m_dampingCoefficient = 1;
-    m_sleepingThreshold = 0.1;
-    m_useFaceContact = true;
+	m_dampingCoefficient = 1.0;
+	m_sleepingThreshold = .4;
 	m_useSelfCollision = false;
-    m_collisionFlags = 0;
+	m_collisionFlags = 0;
+	m_softSoftCollision = false;
+	m_maxSpeedSquared = 0;
+	m_repulsionStiffness = 0.5;
+	m_fdbvnt = 0;
 }
 
 //
@@ -134,6 +243,8 @@ btSoftBody::~btSoftBody()
 		btAlignedFree(m_materials[i]);
 	for (i = 0; i < m_joints.size(); ++i)
 		btAlignedFree(m_joints[i]);
+	if (m_fdbvnt)
+		delete m_fdbvnt;
 }
 
 //
@@ -896,6 +1007,71 @@ void btSoftBody::setVolumeDensity(btScalar density)
 	setVolumeMass(volume * density / 6);
 }
 
+//
+btVector3 btSoftBody::getLinearVelocity()
+{
+    btVector3 total_momentum = btVector3(0,0,0);
+    for (int i = 0; i < m_nodes.size(); ++i)
+    {
+        btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0/m_nodes[i].m_im;
+        total_momentum += mass * m_nodes[i].m_v;
+    }
+    btScalar total_mass = getTotalMass();
+    return total_mass == 0 ? total_momentum : total_momentum / total_mass;
+}
+
+//
+void btSoftBody::setLinearVelocity(const btVector3& linVel)
+{
+    btVector3 old_vel = getLinearVelocity();
+    btVector3 diff = linVel - old_vel;
+    for (int i = 0; i < m_nodes.size(); ++i)
+        m_nodes[i].m_v += diff;
+}
+
+//
+void btSoftBody::setAngularVelocity(const btVector3& angVel)
+{
+    btVector3 old_vel = getLinearVelocity();
+    btVector3 com = getCenterOfMass();
+    for (int i = 0; i < m_nodes.size(); ++i)
+    {
+        m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel;
+    }
+}
+
+//
+btTransform btSoftBody::getRigidTransform()
+{
+    btVector3 t = getCenterOfMass();
+    btMatrix3x3 S;
+    S.setZero();
+    // get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i ||
+    for (int i = 0; i < m_nodes.size(); ++i)
+    {
+        S += OuterProduct(m_X[i], t-m_nodes[i].m_x);
+    }
+    btVector3 sigma;
+    btMatrix3x3 U,V;
+    singularValueDecomposition(S,U,sigma,V);
+    btMatrix3x3 R = V * U.transpose();
+    btTransform trs;
+    trs.setIdentity();
+    trs.setOrigin(t);
+    trs.setBasis(R);
+    return trs;
+}
+
+//
+void btSoftBody::transformTo(const btTransform& trs)
+{
+    // get the current best rigid fit
+    btTransform current_transform = getRigidTransform();
+    // apply transform in material space
+    btTransform new_transform = trs * current_transform.inverse();
+    transform(new_transform);
+}
+
 //
 void btSoftBody::transform(const btTransform& trs)
 {
@@ -916,7 +1092,6 @@ void btSoftBody::transform(const btTransform& trs)
 	updateNormals();
 	updateBounds();
 	updateConstants();
-	m_initialWorldTransform = trs;
 }
 
 //
@@ -1834,6 +2009,25 @@ bool btSoftBody::rayTest(const btVector3& rayFrom,
 	return (rayTest(rayFrom, rayTo, results.fraction, results.feature, results.index, false) != 0);
 }
 
+bool btSoftBody::rayFaceTest(const btVector3& rayFrom,
+                         const btVector3& rayTo,
+                         sRayCast& results)
+{
+	if (m_faces.size() == 0)
+		return false;
+	else
+	{
+    	if (m_fdbvt.empty())
+        	initializeFaceTree();
+	}
+    
+    results.body = this;
+    results.fraction = 1.f;
+    results.index = -1;
+    
+    return (rayFaceTest(rayFrom, rayTo, results.fraction,  results.index) != 0);
+}
+
 //
 void btSoftBody::setSolver(eSolverPresets::_ preset)
 {
@@ -2339,15 +2533,160 @@ int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo,
 	return (cnt);
 }
 
+int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
+						btScalar& mint, int& index) const
+{
+	int cnt = 0;
+	{ /* Use dbvt	*/
+		RayFromToCaster collider(rayFrom, rayTo, mint);
+		
+		btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider);
+		if (collider.m_face)
+		{
+			mint = collider.m_mint;
+			index = (int)(collider.m_face - &m_faces[0]);
+			cnt = 1;
+		}
+	}
+	return (cnt);
+}
+
+
 //
+static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
+{
+	if (n == 0)
+		return 0;
+	btDbvntNode* root = new btDbvntNode(n);
+	if (n->isinternal())
+	{
+		btDbvntNode* c0 = copyToDbvnt(n->childs[0]);
+		root->childs[0] = c0;
+		btDbvntNode* c1 = copyToDbvnt(n->childs[1]);
+		root->childs[1] = c1;
+	}
+	return root;
+}
+
+static inline void calculateNormalCone(btDbvntNode* root)
+{
+	if (!root)
+		return;
+	if (root->isleaf())
+	{
+		const btSoftBody::Face* face = (btSoftBody::Face*)root->data;
+		root->normal = face->m_normal;
+		root->angle = 0;
+	}
+	else
+	{
+		btVector3 n0(0,0,0), n1(0,0,0);
+		btScalar a0 = 0, a1 = 0;
+		if (root->childs[0])
+		{
+			calculateNormalCone(root->childs[0]);
+			n0 = root->childs[0]->normal;
+			a0 = root->childs[0]->angle;
+		}
+		if (root->childs[1])
+		{
+			calculateNormalCone(root->childs[1]);
+			n1 = root->childs[1]->normal;
+			a1 = root->childs[1]->angle;
+		}
+		root->normal = (n0+n1).safeNormalize();
+		root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5;
+	}
+}
+
 void btSoftBody::initializeFaceTree()
 {
+	BT_PROFILE("btSoftBody::initializeFaceTree");
 	m_fdbvt.clear();
+	// create leaf nodes;
+	btAlignedObjectArray<btDbvtNode*> leafNodes;
+	leafNodes.resize(m_faces.size());
 	for (int i = 0; i < m_faces.size(); ++i)
 	{
 		Face& f = m_faces[i];
-		f.m_leaf = m_fdbvt.insert(VolumeOf(f, 0), &f);
+		ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = VolumeOf(f, 0);
+		btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
+		node->parent = NULL;
+		node->data = &f;
+		node->childs[1] = 0;
+		node->volume = vol;
+		leafNodes[i] = node;
+		f.m_leaf = node;
 	}
+	btAlignedObjectArray<btAlignedObjectArray<int> > adj;
+	adj.resize(m_faces.size());
+	// construct the adjacency list for triangles
+	for (int i = 0; i < adj.size(); ++i)
+	{
+		for (int j = i+1; j < adj.size(); ++j)
+		{
+			int dup = 0;
+			for (int k = 0; k < 3; ++k)
+			{
+				for (int l = 0; l < 3; ++l)
+				{
+					if (m_faces[i].m_n[k] == m_faces[j].m_n[l])
+					{
+						++dup;
+						break;
+					}
+				}
+				if (dup == 2)
+				{
+					adj[i].push_back(j);
+					adj[j].push_back(i);
+				}
+			}
+		}
+	}
+	m_fdbvt.m_root = buildTreeBottomUp(leafNodes, adj);
+	if (m_fdbvnt)
+		delete m_fdbvnt;
+	m_fdbvnt = copyToDbvnt(m_fdbvt.m_root);
+	updateFaceTree(false, false);
+	rebuildNodeTree();
+}
+
+//
+void btSoftBody::rebuildNodeTree()
+{
+	m_ndbvt.clear();
+	btAlignedObjectArray<btDbvtNode*> leafNodes;
+	leafNodes.resize(m_nodes.size());
+	for (int i = 0; i < m_nodes.size(); ++i)
+	{
+		Node& n = m_nodes[i];
+		ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = btDbvtVolume::FromCR(n.m_x, 0);
+		btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
+		node->parent = NULL;
+		node->data = &n;
+		node->childs[1] = 0;
+		node->volume = vol;
+		leafNodes[i] = node;
+		n.m_leaf = node;
+	}
+	btAlignedObjectArray<btAlignedObjectArray<int> > adj;
+	adj.resize(m_nodes.size());
+	btAlignedObjectArray<int> old_id;
+	old_id.resize(m_nodes.size());
+	for (int i = 0; i < m_nodes.size(); ++i)
+		old_id[i] = m_nodes[i].index;
+	for (int i = 0; i < m_nodes.size(); ++i)
+		m_nodes[i].index = i;
+	for (int i = 0; i < m_links.size(); ++i)
+	{
+		Link& l = m_links[i];
+		adj[l.m_n[0]->index].push_back(l.m_n[1]->index);
+		adj[l.m_n[1]->index].push_back(l.m_n[0]->index);
+	}
+	m_ndbvt.m_root = buildTreeBottomUp(leafNodes, adj);
+	for (int i = 0; i < m_nodes.size(); ++i)
+		m_nodes[i].index = old_id[i];
 }
 
 //
@@ -2403,10 +2742,9 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
     const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
     // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
     // but resolve contact at x_n
-//    btTransform wtr = (predict) ?
-//    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
-//                 : colObjWrap->getWorldTransform();
-    const btTransform& wtr = colObjWrap->getWorldTransform();
+    btTransform wtr = (predict) ?
+    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+                 : colObjWrap->getWorldTransform();
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(
 			wtr.invXform(x),
@@ -2457,7 +2795,6 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
     btTransform wtr = (predict) ?
     (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
     : colObjWrap->getWorldTransform();
-//    const btTransform& wtr = colObjWrap->getWorldTransform();
     btScalar dst;
     
 //#define USE_QUADRATURE 1
@@ -2476,6 +2813,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
                                           nrm,
                                           margin);
         nrm = wtr.getBasis() * nrm;
+        cti.m_colObj = colObjWrap->getCollisionObject();
         // use cached contact point
     }
     else
@@ -2492,10 +2830,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
         contact_point = results.witnesses[0];
         getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
         nrm = results.normal;
+        cti.m_colObj = colObjWrap->getCollisionObject();
         for (int i = 0; i < 3; ++i)
             f.m_pcontact[i] = bary[i];
     }
-
+    return (dst < 0);
 #endif
 
     // use collision quadrature point
@@ -2505,7 +2844,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
         btVector3 local_nrm;
         for (int q = 0; q < m_quads.size(); ++q)
         {
-            btVector3 p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
+            btVector3 p;
+            if (predict)
+                p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]);
+            else
+                p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
             btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate(
                                                     wtr.invXform(p),
                                                     shp,
@@ -2513,43 +2856,83 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
                                                     margin);
             if (local_dst < dst)
             {
+                if (local_dst < 0 && predict)
+                    return true;
                 dst = local_dst;
                 contact_point = p;
                 bary = m_quads[q];
-                nrm = wtr.getBasis() * local_nrm;
+                nrm = local_nrm;
+            }
+            if (!predict)
+            {
+                cti.m_colObj = colObjWrap->getCollisionObject();
+                cti.m_normal = wtr.getBasis() * nrm;
+                cti.m_offset = dst;
             }
         }
+        return (dst < 0);
     }
 #endif
     
+//    // regular face contact
+//    {
+//        btGjkEpaSolver2::sResults results;
+//        btTransform triangle_transform;
+//        triangle_transform.setIdentity();
+//        triangle_transform.setOrigin(f.m_n[0]->m_x);
+//        btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
+//        btVector3 guess(0,0,0);
+//        if (predict)
+//        {
+//            triangle_transform.setOrigin(f.m_n[0]->m_q);
+//            triangle = btTriangleShape(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q);
+//        }
+//        const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
+////        btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
+////        dst = results.distance - margin;
+////        contact_point = results.witnesses[0];
+//        btGjkEpaSolver2::Penetration(&triangle, triangle_transform, csh, wtr, guess, results);
+//        if (results.status == btGjkEpaSolver2::sResults::Separated)
+//            return false;
+//        dst = results.distance - margin;
+//        contact_point = results.witnesses[1];
+//        getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
+//        nrm = results.normal;
+//        for (int i = 0; i < 3; ++i)
+//            f.m_pcontact[i] = bary[i];
+//    }
+//
+//    if (!predict)
+//    {
+//        cti.m_colObj = colObjWrap->getCollisionObject();
+//        cti.m_normal = nrm;
+//        cti.m_offset = dst;
+//    }
+//
+    
     // regular face contact
     {
         btGjkEpaSolver2::sResults results;
         btTransform triangle_transform;
         triangle_transform.setIdentity();
-        triangle_transform.setOrigin(f.m_n[0]->m_x);
-        btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
+        triangle_transform.setOrigin(f.m_n[0]->m_q);
+        btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q);
         btVector3 guess(0,0,0);
         const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
         btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
-        dst = results.distance - margin;
+        dst = results.distance-csh->getMargin();
+        dst -= margin;
+        if (dst >= 0)
+            return false;
         contact_point = results.witnesses[0];
-        getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
+        getBarycentric(contact_point, f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, bary);
+        btVector3 curr = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
         nrm = results.normal;
-        for (int i = 0; i < 3; ++i)
-            f.m_pcontact[i] = bary[i];
-    }
-    
-    if (!predict)
-    {
         cti.m_colObj = colObjWrap->getCollisionObject();
         cti.m_normal = nrm;
-        cti.m_offset = dst;
+        cti.m_offset = dst + (curr - contact_point).dot(nrm);
     }
-    
-    if (dst < 0)
-        return true;
-    return (false);
+    return (dst < 0);
 }
 
 //
@@ -3075,6 +3458,7 @@ void btSoftBody::setSpringStiffness(btScalar k)
     {
         m_links[i].Feature::m_material->m_kLST = k;
     }
+    m_repulsionStiffness = k;
 }
 
 void btSoftBody::initializeDmInverse()
@@ -3372,18 +3756,39 @@ void btSoftBody::setMaxStress(btScalar maxStress)
 //
 void btSoftBody::interpolateRenderMesh()
 {
-    for (int i = 0; i < m_renderNodes.size(); ++i)
-    {
-        Node& n = m_renderNodes[i];
-        n.m_x.setZero();
-        for (int j = 0; j < 4; ++j)
-        {
-			if (m_renderNodesParents[i].size())
+	if (m_z.size() > 0)
+	{
+		for (int i = 0; i < m_renderNodes.size(); ++i)
+		{
+			const Node* p0 = m_renderNodesParents[i][0];
+			const Node* p1 = m_renderNodesParents[i][1];
+			const Node* p2 = m_renderNodesParents[i][2];
+			btVector3 normal = btCross(p1->m_x - p0->m_x, p2->m_x - p0->m_x);
+			btVector3 unit_normal = normal.normalized();
+			Node& n = m_renderNodes[i];
+			n.m_x.setZero();
+			for (int j = 0; j < 3; ++j)
 			{
 				n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
 			}
-        }
-    }
+			n.m_x += m_z[i] * unit_normal;
+		}
+	}
+	else
+	{
+		for (int i = 0; i < m_renderNodes.size(); ++i)
+		{
+			Node& n = m_renderNodes[i];
+			n.m_x.setZero();
+			for (int j = 0; j < 4; ++j)
+			{
+				if (m_renderNodesParents[i].size())
+				{
+					n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
+				}
+			}
+		}
+	}
 }
 
 void btSoftBody::setCollisionQuadrature(int N)
@@ -3649,13 +4054,10 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
 		break;
         case fCollision::SDF_RD:
         {
-            
             btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
             if (pcoWrap->getCollisionObject()->isActive() || this->isActive())
             {
                 const btTransform wtr = pcoWrap->getWorldTransform();
-//                const btTransform ctr = pcoWrap->getWorldTransform();
-//                const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
                 const btScalar timemargin = 0;
                 const btScalar basemargin = getCollisionShape()->getMargin();
                 btVector3 mins;
@@ -3667,22 +4069,25 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
                                                       maxs);
                 volume = btDbvtVolume::FromMM(mins, maxs);
                 volume.Expand(btVector3(basemargin, basemargin, basemargin));
-                btSoftColliders::CollideSDF_RD docollideNode;
-                docollideNode.psb = this;
-                docollideNode.m_colObj1Wrap = pcoWrap;
-                docollideNode.m_rigidBody = prb1;
-                docollideNode.dynmargin = basemargin + timemargin;
-                docollideNode.stamargin = basemargin;
-                m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
-                
-                if (this->m_useFaceContact)
+				if (m_cfg.collisions & fCollision::SDF_RDN)
+				{
+					btSoftColliders::CollideSDF_RD docollideNode;
+					docollideNode.psb = this;
+					docollideNode.m_colObj1Wrap = pcoWrap;
+					docollideNode.m_rigidBody = prb1;
+					docollideNode.dynmargin = basemargin + timemargin;
+					docollideNode.stamargin = basemargin;
+					m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
+				}
+
+                if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF)))
                 {
                     btSoftColliders::CollideSDF_RDF docollideFace;
                     docollideFace.psb = this;
                     docollideFace.m_colObj1Wrap = pcoWrap;
                     docollideFace.m_rigidBody = prb1;
-                    docollideFace.dynmargin = basemargin + timemargin;
-                    docollideFace.stamargin = basemargin;
+					docollideFace.dynmargin = basemargin + timemargin;
+					docollideFace.stamargin = basemargin;
                     m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
                 }
             }
@@ -3691,51 +4096,6 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
 	}
 }
 
-static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
-{
-    if (n == 0)
-        return 0;
-    btDbvntNode* root = new btDbvntNode(n);
-    if (n->isinternal())
-    {
-        btDbvntNode* c0 = copyToDbvnt(n->childs[0]);
-        root->childs[0] = c0;
-        btDbvntNode* c1 = copyToDbvnt(n->childs[1]);
-        root->childs[1] = c1;
-    }
-    return root;
-}
-
-static inline void calculateNormalCone(btDbvntNode* root)
-{
-    if (!root)
-        return;
-    if (root->isleaf())
-    {
-        const btSoftBody::Face* face = (btSoftBody::Face*)root->data;
-        root->normal = face->m_normal;
-        root->angle = 0;
-    }
-    else
-    {
-        btVector3 n0(0,0,0), n1(0,0,0);
-        btScalar a0 = 0, a1 = 0;
-        if (root->childs[0])
-        {
-            calculateNormalCone(root->childs[0]);
-            n0 = root->childs[0]->normal;
-            a0 = root->childs[0]->angle;
-        }
-        if (root->childs[1])
-        {
-            calculateNormalCone(root->childs[1]);
-            n1 = root->childs[1]->normal;
-            a1 = root->childs[1]->angle;
-        }
-        root->normal = (n0+n1).safeNormalize();
-        root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5;
-    }
-}
 //
 void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 {
@@ -3779,6 +4139,8 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 		break;
         case fCollision::VF_DD:
         {
+            if (!psb->m_softSoftCollision)
+                return;
             if (psb->isActive() || this->isActive())
             {
                 if (this != psb)
@@ -3797,6 +4159,7 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
                     docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
                                                         docollide.psb[1]->m_fdbvt.m_root,
                                                         docollide);
+
                     /* psb1 nodes vs psb0 faces    */
                     if (this->m_tetras.size() > 0)
                         docollide.useFaceNormal = true;
@@ -3812,20 +4175,17 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
                 {
                     if (psb->useSelfCollision())
                     {
-                      btSoftColliders::CollideFF_DD docollide;
-                      docollide.mrg = getCollisionShape()->getMargin() +
-                      psb->getCollisionShape()->getMargin();
-                      docollide.psb[0] = this;
-                      docollide.psb[1] = psb;
-                      if (this->m_tetras.size() > 0)
-                          docollide.useFaceNormal = true;
-                      else
-                          docollide.useFaceNormal = false;
-                      /* psb0 faces vs psb0 faces    */
-                      btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root);
-                      calculateNormalCone(root);
-                      this->m_fdbvt.selfCollideT(root,docollide);
-                      delete root;
+                        btSoftColliders::CollideFF_DD docollide;
+                        docollide.mrg = 2*getCollisionShape()->getMargin();
+                        docollide.psb[0] = this;
+                        docollide.psb[1] = psb;
+                        if (this->m_tetras.size() > 0)
+                            docollide.useFaceNormal = true;
+                        else
+                            docollide.useFaceNormal = false;
+                        /* psb0 faces vs psb0 faces    */
+                        calculateNormalCone(this->m_fdbvnt);
+                        this->m_fdbvt.selfCollideT(m_fdbvnt,docollide);
                     }
                 }
             }
@@ -3837,6 +4197,58 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 	}
 }
 
+void btSoftBody::geometricCollisionHandler(btSoftBody* psb)
+{
+	if (psb->isActive() || this->isActive())
+	{
+		if (this != psb)
+		{
+			btSoftColliders::CollideCCD docollide;
+			/* common                    */
+			docollide.mrg = SAFE_EPSILON; // for rounding error instead of actual margin
+			docollide.dt = psb->m_sst.sdt;
+			/* psb0 nodes vs psb1 faces    */
+			if (psb->m_tetras.size() > 0)
+				docollide.useFaceNormal = true;
+			else
+				docollide.useFaceNormal = false;
+			docollide.psb[0] = this;
+			docollide.psb[1] = psb;
+			docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+												 docollide.psb[1]->m_fdbvt.m_root,
+												 docollide);
+			/* psb1 nodes vs psb0 faces    */
+			if (this->m_tetras.size() > 0)
+				docollide.useFaceNormal = true;
+			else
+				docollide.useFaceNormal = false;
+			docollide.psb[0] = psb;
+			docollide.psb[1] = this;
+			docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+												 docollide.psb[1]->m_fdbvt.m_root,
+												 docollide);
+		}
+		else
+		{
+			if (psb->useSelfCollision())
+			{
+				btSoftColliders::CollideCCD docollide;
+				docollide.mrg = SAFE_EPSILON;
+				docollide.psb[0] = this;
+				docollide.psb[1] = psb;
+                docollide.dt = psb->m_sst.sdt;
+				if (this->m_tetras.size() > 0)
+					docollide.useFaceNormal = true;
+				else
+					docollide.useFaceNormal = false;
+				/* psb0 faces vs psb0 faces    */
+				calculateNormalCone(this->m_fdbvnt);  // should compute this outside of this scope
+				this->m_fdbvt.selfCollideT(m_fdbvnt,docollide);
+			}
+		}
+	}
+}
+
 void btSoftBody::setWindVelocity(const btVector3& velocity)
 {
 	m_windVelocity = velocity;

thirdparty/bullet/BulletSoftBody/btSoftBody.h

@@ -35,6 +35,8 @@ subject to the following restrictions:
 //#else
 #define btSoftBodyData btSoftBodyFloatData
 #define btSoftBodyDataName "btSoftBodyFloatData"
+static const btScalar  OVERLAP_REDUCTION_FACTOR = 0.1;
+static unsigned long seed = 243703;
 //#endif //BT_USE_DOUBLE_PRECISION
 
 class btBroadphaseInterface;
@@ -161,14 +163,18 @@ public:
 			RVSmask = 0x000f,  ///Rigid versus soft mask
 			SDF_RS = 0x0001,   ///SDF based rigid vs soft
 			CL_RS = 0x0002,    ///Cluster vs convex rigid vs soft
-            SDF_RD = 0x0003,   ///DF based rigid vs deformable
-            SDF_RDF = 0x0004,   ///DF based rigid vs deformable faces
+			SDF_RD = 0x0004,   ///rigid vs deformable
 
-			SVSmask = 0x00F0,  ///Rigid versus soft mask
+			SVSmask = 0x00f0,  ///Rigid versus soft mask
 			VF_SS = 0x0010,    ///Vertex vs face soft vs soft handling
 			CL_SS = 0x0020,    ///Cluster vs cluster soft vs soft handling
 			CL_SELF = 0x0040,  ///Cluster soft body self collision
-            VF_DD = 0x0050,    ///Vertex vs face soft vs soft handling
+			VF_DD = 0x0080,    ///Vertex vs face soft vs soft handling
+
+			RVDFmask = 0x0f00, /// Rigid versus deformable face mask
+			SDF_RDF = 0x0100,  /// GJK based Rigid vs. deformable face
+			SDF_MDF = 0x0200,  /// GJK based Multibody vs. deformable face
+            SDF_RDN = 0x0400,  /// SDF based Rigid vs. deformable node
 			/* presets	*/
 			Default = SDF_RS,
 			END
@@ -257,13 +263,13 @@ public:
 		btVector3 m_x;       // Position
 		btVector3 m_q;       // Previous step position/Test position
 		btVector3 m_v;       // Velocity
-        btVector3 m_vsplit;  // Temporary Velocity in addintion to velocity used in split impulse
         btVector3 m_vn;      // Previous step velocity
 		btVector3 m_f;       // Force accumulator
 		btVector3 m_n;       // Normal
 		btScalar m_im;       // 1/mass
 		btScalar m_area;     // Area
 		btDbvtNode* m_leaf;  // Leaf data
+		btScalar m_penetration;   // depth of penetration
 		int m_battach : 1;   // Attached
         int index;
 	};
@@ -289,6 +295,7 @@ public:
 		btScalar m_ra;       // Rest area
 		btDbvtNode* m_leaf;  // Leaf data
         btVector4 m_pcontact; // barycentric weights of the persistent contact
+        btVector3 m_n0, m_n1, m_vn;
         int m_index;
 	};
 	/* Tetra		*/
@@ -717,6 +724,15 @@ public:
 	/* SolverState	*/
 	struct SolverState
 	{
+		//if you add new variables, always initialize them!
+		SolverState()
+			:sdt(0),
+			isdt(0),
+			velmrg(0),
+			radmrg(0),
+			updmrg(0)
+		{
+		}
 		btScalar sdt;     // dt*timescale
 		btScalar isdt;    // 1/sdt
 		btScalar velmrg;  // velocity margin
@@ -796,22 +812,24 @@ public:
 	bool m_bUpdateRtCst;               // Update runtime constants
 	btDbvt m_ndbvt;                    // Nodes tree
 	btDbvt m_fdbvt;                    // Faces tree
+	btDbvntNode* m_fdbvnt;              // Faces tree with normals
 	btDbvt m_cdbvt;                    // Clusters tree
 	tClusterArray m_clusters;          // Clusters
-    btScalar m_dampingCoefficient;     // Damping Coefficient
-    btScalar m_sleepingThreshold;
-    btScalar m_maxSpeedSquared;
-    bool m_useFaceContact;
-    btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
-    
-    btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
-    btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
-    bool m_useSelfCollision;
+	btScalar m_dampingCoefficient;     // Damping Coefficient
+	btScalar m_sleepingThreshold;
+	btScalar m_maxSpeedSquared;
+	btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
+	btScalar m_repulsionStiffness;
+    btAlignedObjectArray<btVector3> m_X;   // initial positions
+
+	btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
+	btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
+	btAlignedObjectArray<btScalar> m_z; // vertical distance used in extrapolation
+	bool m_useSelfCollision;
+	bool m_softSoftCollision;
 
 	btAlignedObjectArray<bool> m_clusterConnectivity;  //cluster connectivity, for self-collision
 
-	btTransform m_initialWorldTransform;
-
 	btVector3 m_windVelocity;
 
 	btScalar m_restLengthScale;
@@ -843,11 +861,6 @@ public:
     {
         m_dampingCoefficient = damping_coeff;
     }
-    
-    void setUseFaceContact(bool useFaceContact)
-    {
-        m_useFaceContact = false;
-    }
 
 	///@todo: avoid internal softbody shape hack and move collision code to collision library
 	virtual void setCollisionShape(btCollisionShape* collisionShape)
@@ -957,6 +970,16 @@ public:
 	void setVolumeMass(btScalar mass);
 	/* Set volume density (using tetrahedrons)								*/
 	void setVolumeDensity(btScalar density);
+	/* Get the linear velocity of the center of mass                        */
+	btVector3 getLinearVelocity();
+	/* Set the linear velocity of the center of mass                        */
+	void setLinearVelocity(const btVector3& linVel);
+	/* Set the angular velocity of the center of mass                       */
+	void setAngularVelocity(const btVector3& angVel);
+    /* Get best fit rigid transform                                         */
+    btTransform getRigidTransform();
+    /* Transform to given pose                                              */
+    void transformTo(const btTransform& trs);
 	/* Transform															*/
 	void transform(const btTransform& trs);
 	/* Translate															*/
@@ -1023,6 +1046,11 @@ public:
 	bool rayTest(const btVector3& rayFrom,
 				 const btVector3& rayTo,
 				 sRayCast& results);
+	bool rayFaceTest(const btVector3& rayFrom,
+					 const btVector3& rayTo,
+					 sRayCast& results);
+	int rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
+					btScalar& mint, int& index) const;
 	/* Solver presets														*/
 	void setSolver(eSolverPresets::_ preset);
 	/* predictMotion														*/
@@ -1120,6 +1148,7 @@ public:
 	int rayTest(const btVector3& rayFrom, const btVector3& rayTo,
 				btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const;
 	void initializeFaceTree();
+	void rebuildNodeTree();
 	btVector3 evaluateCom() const;
 	bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
     bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
@@ -1152,7 +1181,180 @@ public:
 	static void VSolve_Links(btSoftBody* psb, btScalar kst);
 	static psolver_t getSolver(ePSolver::_ solver);
 	static vsolver_t getSolver(eVSolver::_ solver);
+	void geometricCollisionHandler(btSoftBody* psb);
+#define SAFE_EPSILON SIMD_EPSILON*100.0
+	void updateNode(btDbvtNode* node, bool use_velocity, bool margin)
+	{
+		if (node->isleaf())
+		{
+			btSoftBody::Node* n = (btSoftBody::Node*)(node->data);
+			ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
+			btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision
+			if (use_velocity)
+			{
+				btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v};
+				vol = btDbvtVolume::FromPoints(points, 2);
+				vol.Expand(btVector3(pad, pad, pad));
+			}
+			else
+			{
+				vol = btDbvtVolume::FromCR(n->m_x, pad);
+			}
+			node->volume = vol;
+			return;
+		}
+		else
+		{
+			updateNode(node->childs[0], use_velocity, margin);
+			updateNode(node->childs[1], use_velocity, margin);
+			ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
+			Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
+			node->volume = vol;
+		}
+	}
+	
+    void updateNodeTree(bool use_velocity, bool margin)
+	{
+		if (m_ndbvt.m_root)
+			updateNode(m_ndbvt.m_root, use_velocity, margin);
+	}
 
+	template <class DBVTNODE> // btDbvtNode or btDbvntNode
+	void updateFace(DBVTNODE* node, bool use_velocity, bool margin)
+	{
+		if (node->isleaf())
+		{
+			btSoftBody::Face* f = (btSoftBody::Face*)(node->data);
+			btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision
+			ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
+			if (use_velocity)
+			{
+				btVector3 points[6] = {f->m_n[0]->m_x, f->m_n[0]->m_x + m_sst.sdt * f->m_n[0]->m_v,
+					f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v,
+					f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v};
+				vol = btDbvtVolume::FromPoints(points, 6);
+			}
+			else
+			{
+				btVector3 points[3] = {f->m_n[0]->m_x,
+					f->m_n[1]->m_x,
+					f->m_n[2]->m_x};
+				vol = btDbvtVolume::FromPoints(points, 3);
+			}
+			vol.Expand(btVector3(pad, pad, pad));
+			node->volume = vol;
+			return;
+		}
+		else
+		{
+			updateFace(node->childs[0], use_velocity, margin);
+			updateFace(node->childs[1], use_velocity, margin);
+			ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
+			Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
+			node->volume = vol;
+		}
+	}
+	void updateFaceTree(bool use_velocity, bool margin)
+	{
+		if (m_fdbvt.m_root)
+			updateFace(m_fdbvt.m_root, use_velocity, margin);
+		if (m_fdbvnt)
+			updateFace(m_fdbvnt, use_velocity, margin);
+	}
+
+	template <typename T>
+	static inline T BaryEval(const T& a,
+							 const T& b,
+							 const T& c,
+							 const btVector3& coord)
+	{
+		return (a * coord.x() + b * coord.y() + c * coord.z());
+	}
+
+    void applyRepulsionForce(btScalar timeStep, bool applySpringForce)
+	{
+		btAlignedObjectArray<int> indices;
+		{
+			// randomize the order of repulsive force
+			indices.resize(m_faceNodeContacts.size());
+			for (int i = 0; i < m_faceNodeContacts.size(); ++i)
+				indices[i] = i;
+#define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff)
+			int i, ni;
+
+			for (i = 0, ni = indices.size(); i < ni; ++i)
+			{
+				btSwap(indices[i], indices[NEXTRAND % ni]);
+			}
+		}
+		for (int k = 0; k < m_faceNodeContacts.size(); ++k)
+		{
+			int i = indices[k];
+			btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[i];
+			btSoftBody::Node* node = c.m_node;
+			btSoftBody::Face* face = c.m_face;
+			const btVector3& w = c.m_bary;
+			const btVector3& n = c.m_normal;
+			btVector3 l = node->m_x - BaryEval(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, w);
+			btScalar d = c.m_margin - n.dot(l);
+			d = btMax(btScalar(0),d);
+			
+			const btVector3& va = node->m_v;
+			btVector3 vb = BaryEval(face->m_n[0]->m_v, face->m_n[1]->m_v, face->m_n[2]->m_v, w);
+			btVector3 vr = va - vb;
+			const btScalar vn = btDot(vr, n); // dn < 0 <==> opposing
+			if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep)
+				continue;
+			btVector3 vt = vr - vn*n;
+			btScalar I = 0;
+			btScalar mass = node->m_im == 0 ? 0 : btScalar(1)/node->m_im;
+			if (applySpringForce)
+				I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
+			if (vn < 0)
+				I += 0.5 * mass * vn;
+			btScalar face_penetration = 0, node_penetration = node->m_penetration;
+			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)
+			{
+				for (int j = 0; j < 3; ++j)
+					face->m_n[j]->m_v += w[j]*n*I_tilde*node->m_im;
+			}
+            if (face_penetration >= node_penetration)
+			{
+				node->m_v -= I_tilde*node->m_im*n;
+			}
+			
+			// apply frictional impulse
+			btScalar vt_norm = vt.safeNorm();
+			if (vt_norm > SIMD_EPSILON)
+			{
+				btScalar delta_vn = -2 * I * node->m_im;
+				btScalar mu = c.m_friction;
+				btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0))*vt_norm;
+				I = 0.5 * mass * (vt_norm-vt_new);
+				vt.safeNormalize();
+				I_tilde = .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)
+				{
+					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)
+				{
+					node->m_v -= I_tilde * node->m_im * vt;
+				}
+			}
+		}
+	}
 	virtual int calculateSerializeBufferSize() const;
   
 	///fills the dataBuffer and returns the struct name (and 0 on failure)

thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp

@@ -1300,13 +1300,23 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
         }
         else if (reading_tets)
         {
+			int d;
+			ss >> d;
+			if (d != 4)
+			{
+				printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n");
+				fs.close();
+				return 0;
+			}
             ss.ignore(128, ' '); // ignore "4"
             Index tet;
             tet.resize(4);
             for (size_t i = 0; i < 4; i++)
             {
                 ss >> tet[i];
+                printf("%d ", tet[i]);
             }
+            printf("\n");
             indices[indices_count++] = tet;
         }
     }
@@ -1500,10 +1510,27 @@ void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector
     bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6);
 }
 
+// Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0.
+void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary)
+{
+    btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
+    btScalar d00 = btDot(v0, v0);
+    btScalar d01 = btDot(v0, v1);
+    btScalar d11 = btDot(v1, v1);
+    btScalar d20 = btDot(v2, v0);
+    btScalar d21 = btDot(v2, v1);
+    btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01);
+    bary[1] = (d11 * d20 - d01 * d21) * invDenom;
+    bary[2] = (d00 * d21 - d01 * d20) * invDenom;
+    bary[0] = 1.0 - bary[1] - bary[2];
+    bary[3] = 0;
+}
+
 // Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights
 // If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight
 void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
 {
+    psb->m_z.resize(0);
     psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
     psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
     for (int i = 0; i < psb->m_renderNodes.size(); ++i)
@@ -1513,7 +1540,6 @@ void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
         btVector4 optimal_bary;
         btScalar min_bary_weight = -1e3;
         btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
-        bool found = false;
         for (int j = 0; j < psb->m_tetras.size(); ++j)
         {
             const btSoftBody::Tetra& t = psb->m_tetras[j];
@@ -1544,3 +1570,55 @@ void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
         psb->m_renderNodesParents[i] = optimal_parents;
     }
 }
+
+
+// Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense.
+void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb)
+{
+    psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
+    psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
+    psb->m_z.resize(psb->m_renderNodes.size());
+    for (int i = 0; i < psb->m_renderNodes.size(); ++i)
+    {
+        const btVector3& p = psb->m_renderNodes[i].m_x;
+        btVector4 bary;
+        btVector4 optimal_bary;
+        btScalar min_bary_weight = -SIMD_INFINITY;
+        btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
+        btScalar dist = 0, optimal_dist = 0;
+        for (int j = 0; j < psb->m_faces.size(); ++j)
+        {
+            const btSoftBody::Face& f = psb->m_faces[j];
+            btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x,  f.m_n[2]->m_x - f.m_n[0]->m_x);
+            btVector3 unit_n = n.normalized();
+            dist = (p-f.m_n[0]->m_x).dot(unit_n);
+            btVector3 proj_p = p - dist*unit_n;
+            getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary);
+            btScalar new_min_bary_weight = bary[0];
+            for (int k = 1; k < 3; ++k)
+            {
+                new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
+            }
+
+            // p is out of the current best triangle, we found a traingle that's better
+            bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight<0.);
+            // p is inside of the current best triangle, we found a triangle that's better
+            bool better_than_best_inside = (new_min_bary_weight>=0 &&  min_bary_weight>=0 && btFabs(dist)<btFabs(optimal_dist));
+
+            if (better_than_closest_outisde || better_than_best_inside)
+            {
+                btAlignedObjectArray<const btSoftBody::Node*> parents;
+                parents.push_back(f.m_n[0]);
+                parents.push_back(f.m_n[1]);
+                parents.push_back(f.m_n[2]);
+                optimal_parents = parents;
+                optimal_bary = bary;
+                optimal_dist = dist;
+                min_bary_weight = new_min_bary_weight;
+            }
+        }
+        psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
+        psb->m_renderNodesParents[i] = optimal_parents;
+        psb->m_z[i] = optimal_dist;
+    }
+}

thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h

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

thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h

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

thirdparty/bullet/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp

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

thirdparty/bullet/BulletSoftBody/poly34.cpp

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

thirdparty/bullet/BulletSoftBody/poly34.h

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

thirdparty/bullet/LinearMath/btImplicitQRSVD.h

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

thirdparty/bullet/LinearMath/btMatrix3x3.h

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

thirdparty/bullet/LinearMath/btMatrixX.h

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

thirdparty/bullet/LinearMath/btModifiedGramSchmidt.h

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

thirdparty/bullet/LinearMath/btReducedVector.cpp

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

thirdparty/bullet/LinearMath/btReducedVector.h

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

thirdparty/bullet/btBulletCollisionAll.cpp

@@ -23,6 +23,7 @@
 #include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp"
 #include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp"
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp"
 #include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp"
 #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.cpp"

thirdparty/bullet/btLinearMathAll.cpp

@@ -8,6 +8,7 @@
 #include "LinearMath/btConvexHullComputer.cpp"
 #include "LinearMath/btQuickprof.cpp"
 #include "LinearMath/btThreads.cpp"
+#include "LinearMath/btReducedVector.cpp"
 #include "LinearMath/TaskScheduler/btTaskScheduler.cpp"
 #include "LinearMath/TaskScheduler/btThreadSupportPosix.cpp"
 #include "LinearMath/TaskScheduler/btThreadSupportWin32.cpp"