Merge branch 'master' of https://github.com/erwincoumans/bullet3

examples/DeformableDemo/DeformableMultibody.cpp

@@ -206,15 +206,15 @@ void DeformableMultibody::initPhysics()
 
         psb->getCollisionShape()->setMargin(0.25);
         psb->generateBendingConstraints(2);
-        psb->setTotalMass(5);
+        psb->setTotalMass(1);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
-        psb->m_cfg.kDF = .1;
+        psb->m_cfg.kDF = 2;
         psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         psb->setCollisionFlags(0);
         getDeformableDynamicsWorld()->addSoftBody(psb);
 
-        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2, 0.01, false);
+        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30, 1, true);
         getDeformableDynamicsWorld()->addForce(psb, mass_spring);
         m_forces.push_back(mass_spring);
         

examples/DeformableDemo/DeformableRigid.cpp

@@ -219,7 +219,7 @@ void DeformableRigid::initPhysics()
         psb->setTotalMass(1);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
-        psb->m_cfg.kDF = 2;
+        psb->m_cfg.kDF = .4;
         psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addSoftBody(psb);
         

examples/DeformableDemo/GraspDeformable.cpp

@@ -275,12 +275,12 @@ void GraspDeformable::initPhysics()
 	{
 		char absolute_path[1024];
 		b3BulletDefaultFileIO fileio;
-		fileio.findResourcePath("ditto.vtk", absolute_path, 1024);
+        fileio.findResourcePath("ditto.vtk", absolute_path, 1024);
 		//        fileio.findResourcePath("banana.vtk", absolute_path, 1024);
 		//        fileio.findResourcePath("ball.vtk", absolute_path, 1024);
 		//        fileio.findResourcePath("deformable_crumpled_napkin_sim.vtk", absolute_path, 1024);
 		//        fileio.findResourcePath("single_tet.vtk", absolute_path, 1024);
-		//        fileio.findResourcePath("tube.vtk", absolute_path, 1024);
+//                fileio.findResourcePath("tube.vtk", absolute_path, 1024);
 		//        fileio.findResourcePath("torus.vtk", absolute_path, 1024);
 		//        fileio.findResourcePath("paper_roll.vtk", absolute_path, 1024);
 		//        fileio.findResourcePath("bread.vtk", absolute_path, 1024);

examples/DeformableDemo/Pinch.cpp

@@ -231,7 +231,7 @@ void Pinch::initPhysics()
     btVector3 gravity = btVector3(0, -10, 0);
 	m_dynamicsWorld->setGravity(gravity);
     getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
-    
+	getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
     getDeformableDynamicsWorld()->setSolverCallback(dynamics);
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -425,12 +425,12 @@ public:
 
 			m_robotSim.setGravity(btVector3(0, 0, -10));
 			b3RobotSimulatorLoadDeformableBodyArgs args(2, .01, 0.006);
-			args.m_springElasticStiffness = .1;
+			args.m_springElasticStiffness = 1;
-			args.m_springDampingStiffness = .0004;
+			args.m_springDampingStiffness = .01;
-			args.m_springBendingStiffness = 1;
+			args.m_springBendingStiffness = .1;
-			args.m_frictionCoeff = 1;
+			args.m_frictionCoeff = 10;
 			args.m_useSelfCollision = false;
-//			args.m_useFaceContact = true;
+			args.m_useFaceContact = true;
 			args.m_useBendingSprings = true;
 			args.m_startPosition.setValue(0, 0, 0);
 			args.m_startOrientation.setValue(0, 0, 1, 1);
@@ -476,7 +476,7 @@ public:
 			revoluteJoint2.m_jointType = ePoint2PointType;
 			m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1);
 			m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
-			m_robotSim.setNumSimulationSubSteps(8);
+			m_robotSim.setNumSimulationSubSteps(2);
         }
 
 		if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -8153,7 +8153,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
                 {
                     spring_bending_stiffness = clientCmd.m_loadSoftBodyArguments.m_springBendingStiffness;
                 }
-				btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false, spring_bending_stiffness);
+				btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness);
 				deformWorld->addForce(psb, springForce);
 				m_data->m_lf.push_back(springForce);
 			}
@@ -9353,6 +9353,15 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
 		if (deformWorld)
 		{
 			deformWorld->getWorldInfo().m_gravity = grav;
+			for (int i = 0; i < m_data->m_lf.size(); ++i)
+			{
+				btDeformableLagrangianForce* force = m_data->m_lf[i];
+				if (force->getForceType() == BT_GRAVITY_FORCE)
+				{
+					btDeformableGravityForce* gforce = (btDeformableGravityForce*)force;
+					gforce->m_gravity = grav;
+				}
+			}
 		}
 		
 

examples/pybullet/gym/pybullet_envs/stable_baselines/train.py

@@ -0,0 +1,60 @@
+# Code adapted from https://github.com/araffin/rl-baselines-zoo
+# it requires stable-baselines to be installed
+# Colab Notebook: https://colab.research.google.com/drive/1nZkHO4QTYfAksm9ZTaZ5vXyC7szZxC3F
+# Author: Antonin RAFFIN
+# MIT License
+import argparse
+
+import pybullet_envs
+
+import gym
+import numpy as np
+from stable_baselines import SAC, TD3
+from stable_baselines.common.noise import NormalActionNoise
+
+from utils import TimeFeatureWrapper, EvalCallback
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--algo', help='RL Algorithm (Soft Actor-Critic by default)', default='sac',
+                        type=str, required=False, choices=['sac', 'td3'])
+    parser.add_argument('--env', type=str, default='HalfCheetahBulletEnv-v0', help='environment ID')
+    parser.add_argument('-n', '--n-timesteps', help='Number of training timesteps', default=int(1e6),
+                        type=int)
+    args = parser.parse_args()
+
+    env_id = args.env
+    n_timesteps = args.n_timesteps
+    save_path = '{}_{}'.format(args.algo, env_id)
+
+    # Instantiate and wrap the environment
+    env = TimeFeatureWrapper(gym.make(env_id))
+
+    # Create the evaluation environment and callback
+    eval_env = TimeFeatureWrapper(gym.make(env_id))
+    callback = EvalCallback(eval_env, best_model_save_path=save_path + '_best')
+
+    algo = {
+        'sac': SAC,
+        'td3': TD3
+    }[args.algo]
+
+    n_actions = env.action_space.shape[0]
+
+    # Tuned hyperparameters from https://github.com/araffin/rl-baselines-zoo
+    hyperparams = {
+        'sac': dict(batch_size=256, gamma=0.98, policy_kwargs=dict(layers=[256, 256]),
+                    learning_starts=10000, buffer_size=int(2e5), tau=0.01),
+
+        'td3': dict(batch_size=100, policy_kwargs=dict(layers=[400, 300]),
+                    learning_rate=1e-3, learning_starts=10000, buffer_size=int(1e6),
+                    train_freq=1000, gradient_steps=1000,
+                    action_noise=NormalActionNoise(mean=np.zeros(n_actions), sigma=0.1 * np.ones(n_actions)))
+    }[args.algo]
+
+    model = algo('MlpPolicy', env, verbose=1, **hyperparams)
+    model.learn(n_timesteps, callback=callback)
+
+    print("Saving to {}.zip".format(save_path))
+    model.save(save_path)

examples/pybullet/gym/pybullet_envs/stable_baselines/utils.py

@@ -0,0 +1,114 @@
+# Code adapted from https://github.com/araffin/rl-baselines-zoo
+# it requires stable-baselines to be installed
+# Author: Antonin RAFFIN
+# MIT License
+import gym
+import numpy as np
+from gym.wrappers import TimeLimit
+
+from stable_baselines.common.evaluation import evaluate_policy
+
+
+class TimeFeatureWrapper(gym.Wrapper):
+    """
+    Add remaining time to observation space for fixed length episodes.
+    See https://arxiv.org/abs/1712.00378 and https://github.com/aravindr93/mjrl/issues/13.
+
+    :param env: (gym.Env)
+    :param max_steps: (int) Max number of steps of an episode
+        if it is not wrapped in a TimeLimit object.
+    :param test_mode: (bool) In test mode, the time feature is constant,
+        equal to zero. This allow to check that the agent did not overfit this feature,
+        learning a deterministic pre-defined sequence of actions.
+    """
+    def __init__(self, env, max_steps=1000, test_mode=False):
+        assert isinstance(env.observation_space, gym.spaces.Box)
+        # Add a time feature to the observation
+        low, high = env.observation_space.low, env.observation_space.high
+        low, high= np.concatenate((low, [0])), np.concatenate((high, [1.]))
+        env.observation_space = gym.spaces.Box(low=low, high=high, dtype=np.float32)
+
+        super(TimeFeatureWrapper, self).__init__(env)
+
+        if isinstance(env, TimeLimit):
+            self._max_steps = env._max_episode_steps
+        else:
+            self._max_steps = max_steps
+        self._current_step = 0
+        self._test_mode = test_mode
+
+    def reset(self):
+        self._current_step = 0
+        return self._get_obs(self.env.reset())
+
+    def step(self, action):
+        self._current_step += 1
+        obs, reward, done, info = self.env.step(action)
+        return self._get_obs(obs), reward, done, info
+
+    def _get_obs(self, obs):
+        """
+        Concatenate the time feature to the current observation.
+
+        :param obs: (np.ndarray)
+        :return: (np.ndarray)
+        """
+        # Remaining time is more general
+        time_feature = 1 - (self._current_step / self._max_steps)
+        if self._test_mode:
+            time_feature = 1.0
+        # Optionnaly: concatenate [time_feature, time_feature ** 2]
+        return np.concatenate((obs, [time_feature]))
+
+
+class EvalCallback(object):
+    """
+    Callback for evaluating an agent.
+
+    :param eval_env: (gym.Env) The environment used for initialization
+    :param n_eval_episodes: (int) The number of episodes to test the agent
+    :param eval_freq: (int) Evaluate the agent every eval_freq call of the callback.
+    :param deterministic: (bool)
+    :param best_model_save_path: (str)
+    :param verbose: (int)
+    """
+    def __init__(self, eval_env, n_eval_episodes=5, eval_freq=10000,
+                 deterministic=True, best_model_save_path=None, verbose=1):
+        super(EvalCallback, self).__init__()
+        self.n_eval_episodes = n_eval_episodes
+        self.eval_freq = eval_freq
+        self.best_mean_reward = -np.inf
+        self.deterministic = deterministic
+        self.eval_env = eval_env
+        self.verbose = verbose
+        self.model, self.num_timesteps = None, 0
+        self.best_model_save_path = best_model_save_path
+        self.n_calls = 0
+
+    def __call__(self, locals_, globals_):
+        """
+        :param locals_: (dict)
+        :param globals_: (dict)
+        :return: (bool)
+        """
+        self.n_calls += 1
+        self.model = locals_['self']
+        self.num_timesteps = self.model.num_timesteps
+
+        if self.n_calls % self.eval_freq == 0:
+            episode_rewards, _ = evaluate_policy(self.model, self.eval_env, n_eval_episodes=self.n_eval_episodes,
+                                                 deterministic=self.deterministic, return_episode_rewards=True)
+
+
+            mean_reward, std_reward = np.mean(episode_rewards), np.std(episode_rewards)
+            if self.verbose > 0:
+                print("Eval num_timesteps={}, "
+                      "episode_reward={:.2f} +/- {:.2f}".format(self.num_timesteps, mean_reward, std_reward))
+
+            if mean_reward > self.best_mean_reward:
+                if self.best_model_save_path is not None:
+                    print("Saving best model")
+                    self.model.save(self.best_model_save_path)
+                self.best_mean_reward = mean_reward
+
+        return True

setup.py

@@ -491,7 +491,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
 
 setup(
     name='pybullet',
-    version='2.6.3',
+    version='2.6.4',
     description=
     'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
     long_description=

src/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
+	// merge new manifolds, if any
-	m_manifoldsPtr.resizeNoInitialize(0);
+	for (int i = 0; i < m_batchManifoldsPtr.size(); ++i)
-
-	btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr();
-	for (int i = 0; i < pairCount; ++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)

src/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;
 };

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

src/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
@@ -82,7 +82,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.3);
 		m_globalCfm = btScalar(0.);
 		m_frictionERP = btScalar(0.2);  //positional friction 'anchors' are disabled by default
 		m_frictionCFM = btScalar(0.);

src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp

@@ -186,9 +186,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)

src/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -79,7 +79,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)

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -228,16 +228,16 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
     m_objective->reinitialize(nodeUpdated, dt);
 }
 
-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;
 }
 

src/BulletSoftBody/btDeformableBodySolver.h

@@ -65,7 +65,7 @@ public:
     virtual void solveDeformableConstraints(btScalar solverdt);
     
     // solve the contact between deformable and rigid as well as among deformables
-    btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies);
+    btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
     
     // solve the position error  between deformable and rigid as well as among deformables;
     btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
@@ -77,7 +77,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);

src/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,7 +79,7 @@ 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();
@@ -134,14 +134,14 @@ 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.
+    // The magnitude of penetration is the depth of penetration.
-    m_penetration = btMin(btScalar(0), c.m_cti.m_offset);
+    m_penetration = btMin(btScalar(0),c.m_cti.m_offset);
 }
 
 btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
@@ -206,16 +206,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);
+    const 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 +256,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)
     {
@@ -319,9 +321,9 @@ btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSo
 }
 
 /* ================   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)
     {
     }
 
@@ -357,9 +359,9 @@ void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3&
 };
 
 /* ================   Face vs. Rigid   =================== */
-btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
+btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal)
 : m_face(contact.m_face)
-, btDeformableRigidContactConstraint(contact)
+, btDeformableRigidContactConstraint(contact, infoGlobal)
 {
 }
 
@@ -411,19 +413,60 @@ 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 relaxation = 1./btScalar(m_infoGlobal->m_numIterations);
-    btScalar m01 = (btScalar(1)/(im0 + im1));
+	btScalar m01 = (relaxation/(im0 + im1));
-    btScalar m02 = (btScalar(1)/(im0 + im2));
+	btScalar m02 = (relaxation/(im0 + im2));
-    btScalar m12 = (btScalar(1)/(im1 + im2));
+	btScalar m12 = (relaxation/(im1 + im2));
-    
+	#ifdef USE_STRAIN_RATE_LIMITING
-    btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
+	// apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%.
-    btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
+	btScalar p = 0.01;
-    btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
+	btVector3& x0 = face->m_n[0]->m_x;
-    
+	btVector3& x1 = face->m_n[1]->m_x;
-    v0 += dv0;
+	btVector3& x2 = face->m_n[2]->m_x;
-    v1 += dv1;
+	const btVector3 x_diff[3] = {x1-x0, x2-x0, x2-x1};
-    v2 += dv2;
+	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;
 }
 
 void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
@@ -447,11 +490,11 @@ void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3&
 }
 
 /* ================   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 +530,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();

src/BulletSoftBody/btDeformableContactConstraint.h

@@ -24,31 +24,33 @@ public:
     // True if the friction is static
     // False if the friction is dynamic
     bool m_static;
-    
+	const btContactSolverInfo* m_infoGlobal;
-    // normal of the contact
+
-    btVector3 m_normal;
+	// normal of the contact
-    
+	btVector3 m_normal;
-    btDeformableContactConstraint(const btVector3& normal): m_static(false), m_normal(normal)
+
-    {
+	btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal)
-    }
+	{
-    
+	}
-    btDeformableContactConstraint(bool isStatic, const btVector3& normal): m_static(isStatic), m_normal(normal)
+
-    {
+	btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal)
-    }
+	{
-    
+	}
-    btDeformableContactConstraint(const btDeformableContactConstraint& other)
+	
-    : m_static(other.m_static)
+	btDeformableContactConstraint(){}
-    , m_normal(other.m_normal)
+
-    {
+	btDeformableContactConstraint(const btDeformableContactConstraint& other)
-        
+	: m_static(other.m_static)
-    }
+	, m_normal(other.m_normal)
-    btDeformableContactConstraint(){}
+	, 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;
+    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0;
     
     // solve the position error by applying an inelastic impulse that changes only the position (not velocity)
     virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) = 0;
@@ -79,22 +81,19 @@ class btDeformableStaticConstraint : public btDeformableContactConstraint
 public:
     const btSoftBody::Node* m_node;
     
-    btDeformableStaticConstraint(){}
+    btDeformableStaticConstraint(const btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal)
-    
-    btDeformableStaticConstraint(const btSoftBody::Node* node): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0))
     {
     }
-    
+	btDeformableStaticConstraint(){}
     btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
     : m_node(other.m_node)
     , btDeformableContactConstraint(other)
     {
-        
     }
     
     virtual ~btDeformableStaticConstraint(){}
     
-    virtual btScalar solveConstraint()
+    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal)
     {
         return 0;
     }
@@ -130,14 +129,14 @@ class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint
 {
 public:
     const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
-    
+	
-    btDeformableNodeAnchorConstraint(){}
+    btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal);
-    btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c);
     btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
+	btDeformableNodeAnchorConstraint(){}
     virtual ~btDeformableNodeAnchorConstraint()
     {
     }
-    virtual btScalar solveConstraint();
+    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
     virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
     {
         // todo xuchenhan@
@@ -169,10 +168,10 @@ public:
     btVector3 m_total_tangent_dv;
     btScalar m_penetration;
     const btSoftBody::DeformableRigidContact* m_contact;
-    
+	
-    btDeformableRigidContactConstraint(){}
+    btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal);
-    btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c);
     btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other);
+	btDeformableRigidContactConstraint(){}
     virtual ~btDeformableRigidContactConstraint()
     {
     }
@@ -180,7 +179,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 solveConstraint(const btContactSolverInfo& infoGlobal);
     
     virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
     
@@ -197,11 +196,10 @@ class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactCo
 public:
     // the deformable node in contact
     const btSoftBody::Node* m_node;
-    
+	
-    btDeformableNodeRigidContactConstraint(){}
+    btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal);
-    btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact);
     btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other);
-    
+	btDeformableNodeRigidContactConstraint(){}
     virtual ~btDeformableNodeRigidContactConstraint()
     {
     }
@@ -228,10 +226,9 @@ class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactCo
 {
 public:
     const btSoftBody::Face* m_face;
-    btDeformableFaceRigidContactConstraint(){}
+    btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal);
-    btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact);
     btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
-    
+	btDeformableFaceRigidContactConstraint(){}
     virtual ~btDeformableFaceRigidContactConstraint()
     {
     }
@@ -263,13 +260,11 @@ public:
     btVector3 m_total_normal_dv;
     btVector3 m_total_tangent_dv;
     
-    btDeformableFaceNodeContactConstraint(){}
+    btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal);
-    
+	btDeformableFaceNodeContactConstraint(){}
-    btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact);
-    
     virtual ~btDeformableFaceNodeContactConstraint(){}
     
-    virtual btScalar solveConstraint();
+    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
     
     virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
     {

src/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);
 			}
 		}
@@ -108,7 +108,7 @@ btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverI
 	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 +124,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 +139,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 +152,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 +173,7 @@ void btDeformableContactProjection::setConstraints()
 			{
 				continue;
 			}
-			btDeformableFaceRigidContactConstraint constraint(contact);
+			btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal);
 			btVector3 va = constraint.getVa();
 			btVector3 vb = constraint.getVb();
 			const btVector3 vr = vb - va;
@@ -190,7 +190,7 @@ void btDeformableContactProjection::setConstraints()
 		{
 			const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j];
 
-			btDeformableFaceNodeContactConstraint constraint(contact);
+			btDeformableFaceNodeContactConstraint constraint(contact, infoGlobal);
 			btVector3 va = constraint.getVa();
 			btVector3 vb = constraint.getVb();
 			const btVector3 vr = vb - va;

src/BulletSoftBody/btDeformableContactProjection.h

@@ -72,13 +72,13 @@ public:
     virtual void applyDynamicFriction(TVStack& f);
     
     // update and solve the constraints
-    virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies);
+    virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
     
     // solve the position error using split impulse
     virtual btScalar solveSplitImpulse(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();

src/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);
+//                    btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
-                    leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+//                    leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
                 }
                 if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
                 {

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -285,7 +285,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
     {

src/BulletSoftBody/btSoftBody.cpp

@@ -53,6 +53,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV
 		n.m_material = pm;
 	}
 	updateBounds();
+	setCollisionQuadrature(3);
 }
 
 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo)
@@ -2403,10 +2404,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) ?
+    btTransform wtr = (predict) ?
-//    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
-//                 : colObjWrap->getWorldTransform();
+                 : colObjWrap->getWorldTransform();
-    const btTransform& wtr = colObjWrap->getWorldTransform();
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(
 			wtr.invXform(x),
@@ -2457,10 +2457,9 @@ 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
+#define USE_QUADRATURE 1
 //#define CACHE_PREV_COLLISION
     
     // use the contact position of the previous collision
@@ -2476,6 +2475,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 +2492,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 +2506,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,12 +2518,21 @@ 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
     
@@ -2530,6 +2544,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
         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;
@@ -2547,9 +2566,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
         cti.m_offset = dst;
     }
     
-    if (dst < 0)
+    return (dst < 0);
-        return true;
-    return (false);
 }
 
 //
@@ -3681,8 +3698,8 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
                     docollideFace.psb = this;
                     docollideFace.m_colObj1Wrap = pcoWrap;
                     docollideFace.m_rigidBody = prb1;
-                    docollideFace.dynmargin = basemargin + timemargin;
+					docollideFace.dynmargin = 0.05*(basemargin + timemargin);
-                    docollideFace.stamargin = basemargin;
+					docollideFace.stamargin = 0.05*basemargin;
                     m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
                 }
             }

src/BulletSoftBody/btSoftBodyInternals.h

@@ -1070,8 +1070,8 @@ struct btSoftColliders
 
 			if (!n.m_battach)
             {
-                // check for collision at x_{n+1}^* as well at x_n
+				// check for collision at x_{n+1}^*
-                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))
+				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 +1159,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;
@@ -1181,6 +1180,8 @@ struct btSoftColliders
                     // 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