Merge remote-tracking branch 'bp/master'

CMakeLists.txt

@@ -470,6 +470,7 @@ IF (BUILD_UNIT_TESTS)
 ENDIF()
 
 set (BULLET_CONFIG_CMAKE_PATH lib${LIB_SUFFIX}/cmake/bullet )
+list (APPEND BULLET_DEFINITIONS ${BULLET_DOUBLE_DEF})
 list (APPEND BULLET_LIBRARIES LinearMath)
 list (APPEND BULLET_LIBRARIES Bullet3Common)
 list (APPEND BULLET_LIBRARIES BulletInverseDynamics)

examples/CommonInterfaces/CommonDeformableBodyBase.h

@@ -21,11 +21,13 @@ struct CommonDeformableBodyBase : public CommonMultiBodyBase
 	btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
 	btSoftBody* m_pickedSoftBody;
 	btDeformableMousePickingForce* m_mouseForce;
-	btScalar m_maxPickingForce;
+	btScalar m_pickingForceElasticStiffness, m_pickingForceDampingStiffness, m_maxPickingForce;
 	CommonDeformableBodyBase(GUIHelperInterface* helper)
 	: CommonMultiBodyBase(helper),
 	m_pickedSoftBody(0),
 	m_mouseForce(0),
+	m_pickingForceElasticStiffness(100),
+	m_pickingForceDampingStiffness(0.0),
 	m_maxPickingForce(0.3)
 	{
 	}
@@ -115,7 +117,7 @@ struct CommonDeformableBodyBase : public CommonMultiBodyBase
 					m_pickedSoftBody = psb;
 					psb->setActivationState(DISABLE_DEACTIVATION);
 					const btSoftBody::Face& f = psb->m_faces[face_id];
-					btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(100, 0, f, m_hitPos, m_maxPickingForce);
+					btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(m_pickingForceElasticStiffness, m_pickingForceDampingStiffness, f, m_hitPos, m_maxPickingForce);
 					m_mouseForce = mouse_force;
 					getDeformableDynamicsWorld()->addForce(psb, mouse_force);
 				}

examples/DeformableDemo/Collide.cpp

@@ -0,0 +1,273 @@
+/*
+ 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.
+ */
+
+#include "Collide.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The Collide shows the contact between volumetric deformable objects and rigid objects.
+static btScalar E = 50;
+static btScalar nu = 0.3;
+static btScalar damping_alpha = 0.1;
+static btScalar damping_beta = 0.01;
+static btScalar COLLIDING_VELOCITY = 15;
+
+struct TetraCube
+{
+#include "../SoftDemo/cube.inl"
+};
+
+class Collide : public CommonDeformableBodyBase
+{
+	btDeformableLinearElasticityForce* m_linearElasticity;
+
+public:
+	Collide(struct GUIHelperInterface* helper)
+		: CommonDeformableBodyBase(helper)
+	{
+        m_linearElasticity = 0;
+	}
+	virtual ~Collide()
+	{
+	}
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 20;
+        float pitch = 0;
+        float yaw = 90;
+        float targetPos[3] = {0, 3, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+    
+    void Ctor_RbUpStack()
+    {
+        float mass = 0.5;
+        btCollisionShape* shape = new btBoxShape(btVector3(2, 2, 2));
+        btTransform startTransform;
+        startTransform.setIdentity();
+        startTransform.setOrigin(btVector3(0,-2,0));
+        btRigidBody* rb = createRigidBody(mass, startTransform, shape);
+        rb->setLinearVelocity(btVector3(0,+COLLIDING_VELOCITY, 0));
+    }
+    
+    void stepSimulation(float deltaTime)
+    {
+		m_linearElasticity->setPoissonRatio(nu);
+		m_linearElasticity->setYoungsModulus(E);
+		m_linearElasticity->setDamping(damping_alpha, damping_beta);
+        float internalTimeStep = 1. / 60.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
+    }
+    
+    virtual void renderScene()
+    {
+        CommonDeformableBodyBase::renderScene();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
+            {
+                btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+            }
+        }
+    }
+};
+
+void Collide::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+	///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+	m_broadphase = new btDbvtBroadphase();
+    btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
+
+	btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(deformableBodySolver);
+	m_solver = sol;
+
+	m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
+    btVector3 gravity = btVector3(0, 0, 0);
+    m_dynamicsWorld->setGravity(gravity);
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    // create volumetric soft body
+    {
+        btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
+                                                                  TetraCube::getElements(),
+                                                                  0,
+                                                                  TetraCube::getNodes(),
+                                                                  false, true, true);
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+        psb->scale(btVector3(2, 2, 2));
+        psb->translate(btVector3(0, 7, 0));
+        psb->getCollisionShape()->setMargin(0.1);
+        psb->setTotalMass(0.5);
+        psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        psb->m_cfg.kCHR = 1; // collision hardness with rigid body
+		psb->m_cfg.kDF = 0;
+        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+		psb->m_sleepingThreshold = 0;
+        btSoftBodyHelpers::generateBoundaryFaces(psb);
+        
+        psb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
+        
+        btDeformableLinearElasticityForce* linearElasticity = new btDeformableLinearElasticityForce(100,100,0.01);
+		m_linearElasticity = linearElasticity;
+        getDeformableDynamicsWorld()->addForce(psb, linearElasticity);
+        m_forces.push_back(linearElasticity);
+    }
+    getDeformableDynamicsWorld()->setImplicit(true);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    getDeformableDynamicsWorld()->setUseProjection(true);
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
+    getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true;
+    getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
+    // add a few rigid bodies
+    Ctor_RbUpStack();
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+	
+//	{
+//		SliderParams slider("Young's Modulus", &E);
+//		slider.m_minVal = 0;
+//		slider.m_maxVal = 2000;
+//		if (m_guiHelper->getParameterInterface())
+//			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+//	}
+//	{
+//		SliderParams slider("Poisson Ratio", &nu);
+//		slider.m_minVal = 0.05;
+//		slider.m_maxVal = 0.49;
+//		if (m_guiHelper->getParameterInterface())
+//			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+//	}
+//	{
+//		SliderParams slider("Mass Damping", &damping_alpha);
+//		slider.m_minVal = 0.001;
+//		slider.m_maxVal = 0.01;
+//		if (m_guiHelper->getParameterInterface())
+//			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+//	}
+//    {
+//        SliderParams slider("Stiffness Damping", &damping_beta);
+//        slider.m_minVal = 0.001;
+//        slider.m_maxVal = 0.01;
+//        if (m_guiHelper->getParameterInterface())
+//            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+//    }
+    {
+        SliderParams slider("Young's Modulus", &E);
+        slider.m_minVal = 0;
+        slider.m_maxVal = 2000;
+        if (m_guiHelper->getParameterInterface())
+            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+    {
+        SliderParams slider("Poisson Ratio", &nu);
+        slider.m_minVal = 0.05;
+        slider.m_maxVal = 0.49;
+        if (m_guiHelper->getParameterInterface())
+            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+    {
+        SliderParams slider("Mass Damping", &damping_alpha);
+        slider.m_minVal = 0;
+        slider.m_maxVal = 1;
+        if (m_guiHelper->getParameterInterface())
+            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+    {
+        SliderParams slider("Stiffness Damping", &damping_beta);
+        slider.m_minVal = 0;
+        slider.m_maxVal = 0.1;
+        if (m_guiHelper->getParameterInterface())
+            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+}
+
+void Collide::exitPhysics()
+{
+	//cleanup in the reverse order of creation/initialization
+    removePickingConstraint();
+	//remove the rigidbodies from the dynamics world and delete them
+	int i;
+	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+	{
+		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+		btRigidBody* body = btRigidBody::upcast(obj);
+		if (body && body->getMotionState())
+		{
+			delete body->getMotionState();
+		}
+		m_dynamicsWorld->removeCollisionObject(obj);
+		delete obj;
+	}
+    // delete forces
+    for (int j = 0; j < m_forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = m_forces[j];
+        delete force;
+    }
+    m_forces.clear();
+    
+	//delete collision shapes
+	for (int j = 0; j < m_collisionShapes.size(); j++)
+	{
+		btCollisionShape* shape = m_collisionShapes[j];
+		delete shape;
+	}
+	m_collisionShapes.clear();
+
+	delete m_dynamicsWorld;
+
+	delete m_solver;
+
+	delete m_broadphase;
+
+	delete m_dispatcher;
+
+	delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* CollideCreateFunc(struct CommonExampleOptions& options)
+{
+	return new Collide(options.m_guiHelper);
+}
+
+

examples/DeformableDemo/Collide.h

@@ -0,0 +1,19 @@
+/*
+ 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 _COLLIDE_H
+#define _COLLIDE_H
+
+class CommonExampleInterface* CollideCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_COLLIDE_H

examples/DeformableDemo/DeformableSelfCollision.cpp

@@ -31,6 +31,7 @@ public:
     DeformableSelfCollision(struct GUIHelperInterface* helper)
     : CommonDeformableBodyBase(helper)
     {
+        m_maxPickingForce = 0.004;
     }
     virtual ~DeformableSelfCollision()
     {
@@ -93,7 +94,7 @@ void DeformableSelfCollision::initPhysics()
     {
         ///create a ground
         btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(2.5), btScalar(150.)));
-        groundShape->setMargin(0.0001);
+        groundShape->setMargin(0.02);
         m_collisionShapes.push_back(groundShape);
         
         btTransform groundTransform;
@@ -152,6 +153,7 @@ void DeformableSelfCollision::addCloth(btVector3 origin)
     clothTransform.setOrigin(btVector3(0,0.2,0)+origin);
     psb->transform(clothTransform);
     psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+    psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
     psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
     psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
     getDeformableDynamicsWorld()->addSoftBody(psb);
@@ -164,6 +166,7 @@ void DeformableSelfCollision::addCloth(btVector3 origin)
     btVector3 gravity = btVector3(0, -9.8, 0);
     btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(gravity);
     getDeformableDynamicsWorld()->addForce(psb, gravity_force);
+    getDeformableDynamicsWorld()->setUseProjection(true);
     m_forces.push_back(gravity_force);
 }
 

examples/DeformableDemo/LargeDeformation.cpp

@@ -0,0 +1,262 @@
+/*
+ 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.
+ */
+
+#include "LargeDeformation.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The LargeDeformation shows the contact between volumetric deformable objects and rigid objects.
+static btScalar E = 50;
+static btScalar nu = 0.3;
+static btScalar damping_alpha = 0.1;
+static btScalar damping_beta = 0.01;
+
+struct TetraCube
+{
+#include "../SoftDemo/cube.inl"
+};
+
+class LargeDeformation : public CommonDeformableBodyBase
+{
+	btDeformableLinearElasticityForce* m_linearElasticity;
+
+public:
+	LargeDeformation(struct GUIHelperInterface* helper)
+		: CommonDeformableBodyBase(helper)
+	{
+        m_linearElasticity = 0;
+	}
+	virtual ~LargeDeformation()
+	{
+	}
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 20;
+        float pitch = -45;
+        float yaw = 100;
+        float targetPos[3] = {0, 3, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+    
+    void stepSimulation(float deltaTime)
+    {
+		m_linearElasticity->setPoissonRatio(nu);
+		m_linearElasticity->setYoungsModulus(E);
+		m_linearElasticity->setDamping(damping_alpha, damping_beta);
+        float internalTimeStep = 1. / 60.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
+    }
+    
+    virtual void renderScene()
+    {
+        CommonDeformableBodyBase::renderScene();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
+            {
+                btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+            }
+        }
+    }
+};
+
+void LargeDeformation::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+	///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+	m_broadphase = new btDbvtBroadphase();
+    btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
+
+	btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(deformableBodySolver);
+	m_solver = sol;
+
+	m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    // create volumetric soft body
+    {
+        btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
+                                                                  TetraCube::getElements(),
+                                                                  0,
+                                                                  TetraCube::getNodes(),
+                                                                  false, true, true);
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+        psb->scale(btVector3(2, 2, 2));
+        psb->translate(btVector3(0, 5, 0));
+        psb->getCollisionShape()->setMargin(0.1);
+        psb->setTotalMass(0.5);
+        psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        psb->m_cfg.kCHR = 1; // collision hardness with rigid body
+		psb->m_cfg.kDF = 0.5;
+        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+		psb->m_sleepingThreshold = 0;
+        btSoftBodyHelpers::generateBoundaryFaces(psb);
+		for (int i = 0; i < psb->m_nodes.size(); ++i)
+		{
+			for (int j = 0; j < 3; ++j)
+				psb->m_nodes[i].m_x[j] = ((double) 2*rand() / (RAND_MAX))-1.0;
+			psb->m_nodes[i].m_x[1]+=8;
+		}
+        
+        btDeformableLinearElasticityForce* linearElasticity = new btDeformableLinearElasticityForce(100,100,0.01);
+		m_linearElasticity = linearElasticity;
+        getDeformableDynamicsWorld()->addForce(psb, linearElasticity);
+        m_forces.push_back(linearElasticity);
+    }
+    getDeformableDynamicsWorld()->setImplicit(true);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    getDeformableDynamicsWorld()->setUseProjection(true);
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.1;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(20);
+    getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true;
+    getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
+    // add a few rigid bodies
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+	
+    {
+        SliderParams slider("Young's Modulus", &E);
+        slider.m_minVal = 0;
+        slider.m_maxVal = 2000;
+        if (m_guiHelper->getParameterInterface())
+            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+    {
+        SliderParams slider("Poisson Ratio", &nu);
+        slider.m_minVal = 0.05;
+        slider.m_maxVal = 0.49;
+        if (m_guiHelper->getParameterInterface())
+            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+    {
+        SliderParams slider("Mass Damping", &damping_alpha);
+        slider.m_minVal = 0;
+        slider.m_maxVal = 1;
+        if (m_guiHelper->getParameterInterface())
+            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+    {
+        SliderParams slider("Stiffness Damping", &damping_beta);
+        slider.m_minVal = 0;
+        slider.m_maxVal = 0.1;
+        if (m_guiHelper->getParameterInterface())
+            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+//	{
+//		SliderParams slider("Young's Modulus", &E);
+//		slider.m_minVal = 0;
+//		slider.m_maxVal = 200;
+//		if (m_guiHelper->getParameterInterface())
+//			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+//	}
+//	{
+//		SliderParams slider("Poisson Ratio", &nu);
+//		slider.m_minVal = 0.05;
+//		slider.m_maxVal = 0.40;
+//		if (m_guiHelper->getParameterInterface())
+//			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+//	}
+//	{
+//		SliderParams slider("Mass Damping", &damping_alpha);
+//		slider.m_minVal = 0.001;
+//		slider.m_maxVal = 0.01;
+//		if (m_guiHelper->getParameterInterface())
+//			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+//	}
+//    {
+//        SliderParams slider("Stiffness Damping", &damping_beta);
+//        slider.m_minVal = 0.001;
+//        slider.m_maxVal = 0.01;
+//        if (m_guiHelper->getParameterInterface())
+//            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+//    }
+}
+
+void LargeDeformation::exitPhysics()
+{
+	//cleanup in the reverse order of creation/initialization
+    removePickingConstraint();
+	//remove the rigidbodies from the dynamics world and delete them
+	int i;
+	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+	{
+		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+		btRigidBody* body = btRigidBody::upcast(obj);
+		if (body && body->getMotionState())
+		{
+			delete body->getMotionState();
+		}
+		m_dynamicsWorld->removeCollisionObject(obj);
+		delete obj;
+	}
+    // delete forces
+    for (int j = 0; j < m_forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = m_forces[j];
+        delete force;
+    }
+    m_forces.clear();
+    
+	//delete collision shapes
+	for (int j = 0; j < m_collisionShapes.size(); j++)
+	{
+		btCollisionShape* shape = m_collisionShapes[j];
+		delete shape;
+	}
+	m_collisionShapes.clear();
+
+	delete m_dynamicsWorld;
+
+	delete m_solver;
+
+	delete m_broadphase;
+
+	delete m_dispatcher;
+
+	delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* LargeDeformationCreateFunc(struct CommonExampleOptions& options)
+{
+	return new LargeDeformation(options.m_guiHelper);
+}
+
+

examples/DeformableDemo/LargeDeformation.h

@@ -0,0 +1,19 @@
+/*
+ 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 _LARGE_DEFORMATION_H
+#define _LARGE_DEFORMATION_H
+
+class CommonExampleInterface* LargeDeformationCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_LARGE_DEFORMATION_H

examples/DeformableDemo/VolumetricDeformable.cpp

@@ -27,9 +27,10 @@
 #include "../Utils/b3ResourcePath.h"
 
 ///The VolumetricDeformable shows the contact between volumetric deformable objects and rigid objects.
-static btScalar E = 25;
+static btScalar E = 50;
 static btScalar nu = 0.3;
-static btScalar damping = 0.01;
+static btScalar damping_alpha = 0.1;
+static btScalar damping_beta = 0.01;
 
 struct TetraCube
 {
@@ -38,13 +39,16 @@ struct TetraCube
 
 class VolumetricDeformable : public CommonDeformableBodyBase
 {
-	btDeformableNeoHookeanForce* m_neohookean;
+	btDeformableLinearElasticityForce* m_linearElasticity;
 
 public:
 	VolumetricDeformable(struct GUIHelperInterface* helper)
 		: CommonDeformableBodyBase(helper)
 	{
-        m_neohookean = 0;
+        m_linearElasticity = 0;
+		m_pickingForceElasticStiffness = 100;
+		m_pickingForceDampingStiffness = 0;
+		m_maxPickingForce = 1e10; // allow large picking force with implicit scheme.
 	}
 	virtual ~VolumetricDeformable()
 	{
@@ -64,12 +68,12 @@ public:
     
     void stepSimulation(float deltaTime)
     {
-		m_neohookean->setPoissonRatio(nu);
-		m_neohookean->setYoungsModulus(E);
-		m_neohookean->setDamping(damping);
+		m_linearElasticity->setPoissonRatio(nu);
+		m_linearElasticity->setYoungsModulus(E);
+		m_linearElasticity->setDamping(damping_alpha, damping_beta);
         //use a smaller internal timestep, there are stability issues
-        float internalTimeStep = 1. / 480.f;
-        m_dynamicsWorld->stepSimulation(deltaTime, 8, internalTimeStep);
+        float internalTimeStep = 1. / 240;
+        m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
     }
     
     void createStaticBox(const btVector3& halfEdge, const btVector3& translation)
@@ -100,7 +104,7 @@ public:
     
     void Ctor_RbUpStack(int count)
     {
-        float mass = 0.2;
+        float mass = 2;
         
         btCompoundShape* cylinderCompound = new btCompoundShape;
         btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5));
@@ -187,7 +191,7 @@ void VolumetricDeformable::initPhysics()
         btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
         btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
         btRigidBody* body = new btRigidBody(rbInfo);
-        body->setFriction(0.5);
+        body->setFriction(1);
 
         //add the ground to the dynamics world
         m_dynamicsWorld->addRigidBody(body);
@@ -208,29 +212,32 @@ void VolumetricDeformable::initPhysics()
         getDeformableDynamicsWorld()->addSoftBody(psb);
         psb->scale(btVector3(2, 2, 2));
         psb->translate(btVector3(0, 5, 0));
-        psb->getCollisionShape()->setMargin(0.025);
+        psb->getCollisionShape()->setMargin(0.1);
         psb->setTotalMass(0.5);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
-        psb->m_cfg.kDF = 0.5;
+		psb->m_cfg.kDF = 2;
         psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
-        psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
-        psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
+        psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
 		psb->m_sleepingThreshold = 0;
         btSoftBodyHelpers::generateBoundaryFaces(psb);
-        
         btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(gravity);
         getDeformableDynamicsWorld()->addForce(psb, gravity_force);
         m_forces.push_back(gravity_force);
         
-        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(500,2000,0.01);
-		m_neohookean = neohookean;
-        getDeformableDynamicsWorld()->addForce(psb, neohookean);
-        m_forces.push_back(neohookean);
-        
+        btDeformableLinearElasticityForce* linearElasticity = new btDeformableLinearElasticityForce(100,100,0.01);
+		m_linearElasticity = linearElasticity;
+        getDeformableDynamicsWorld()->addForce(psb, linearElasticity);
+        m_forces.push_back(linearElasticity);
     }
-    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setImplicit(true);
     getDeformableDynamicsWorld()->setLineSearch(false);
+    getDeformableDynamicsWorld()->setUseProjection(true);
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
+    getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true;
+    getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
     // add a few rigid bodies
     Ctor_RbUpStack(4);
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
@@ -238,24 +245,31 @@ void VolumetricDeformable::initPhysics()
 	{
 		SliderParams slider("Young's Modulus", &E);
 		slider.m_minVal = 0;
-		slider.m_maxVal = 50;
+		slider.m_maxVal = 2000;
 		if (m_guiHelper->getParameterInterface())
 			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}
 	{
 		SliderParams slider("Poisson Ratio", &nu);
 		slider.m_minVal = 0.05;
-		slider.m_maxVal = 0.40;
+		slider.m_maxVal = 0.49;
 		if (m_guiHelper->getParameterInterface())
 			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}
 	{
-		SliderParams slider("Damping", &damping);
-		slider.m_minVal = 0.01;
-		slider.m_maxVal = 0.02;
+		SliderParams slider("Mass Damping", &damping_alpha);
+		slider.m_minVal = 0;
+		slider.m_maxVal = 1;
 		if (m_guiHelper->getParameterInterface())
 			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}
+    {
+        SliderParams slider("Stiffness Damping", &damping_beta);
+        slider.m_minVal = 0;
+        slider.m_maxVal = 0.1;
+        if (m_guiHelper->getParameterInterface())
+            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
 }
 
 void VolumetricDeformable::exitPhysics()

examples/ExampleBrowser/CMakeLists.txt

@@ -373,6 +373,10 @@ SET(BulletExampleBrowser_SRCS
   ../DeformableDemo/SplitImpulse.h
   ../DeformableDemo/VolumetricDeformable.cpp
   ../DeformableDemo/VolumetricDeformable.h
+  ../DeformableDemo/Collide.cpp
+  ../DeformableDemo/Collide.h
+  ../DeformableDemo/LargeDeformation.cpp
+  ../DeformableDemo/LargeDeformation.h
   ../DeformableDemo/DeformableClothAnchor.cpp
   ../DeformableDemo/DeformableClothAnchor.h
   ../DeformableDemo/MultibodyClothAnchor.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -52,6 +52,8 @@
 #include "../DeformableDemo/PinchFriction.h"
 #include "../DeformableDemo/DeformableMultibody.h"
 #include "../DeformableDemo/VolumetricDeformable.h"
+#include "../DeformableDemo/LargeDeformation.h"
+#include "../DeformableDemo/Collide.h"
 #include "../DeformableDemo/GraspDeformable.h"
 #include "../DeformableDemo/DeformableContact.h"
 #include "../DeformableDemo/DeformableClothAnchor.h"
@@ -202,6 +204,8 @@ static ExampleEntry gDefaultExamples[] =
         ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc),
         ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc),
         ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc),
+		ExampleEntry(1, "Extreme Deformation", "Recovery from extreme deformation", LargeDeformationCreateFunc),
+		ExampleEntry(1, "Colliding Test", "Volumetric deformable collide with rigid box", CollideCreateFunc),
         ExampleEntry(1, "Rigid Cloth Anchor", "Deformable Rigid body Anchor test", DeformableClothAnchorCreateFunc),
         ExampleEntry(1, "Multibody Cloth Anchor", "Deformable Multibody Anchor test", MultibodyClothAnchorCreateFunc),
         ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc),

examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py

@@ -10,16 +10,28 @@ import pybullet_data
 import pybullet as p1
 import random
 
+from enum import Enum
+
+class InitializationStrategy(Enum):
+  """Set how the environment is initialized."""
+  START = 0
+  RANDOM = 1  # random state initialization (RSI)
+
 
 class PyBulletDeepMimicEnv(Env):
 
-  def __init__(self, arg_parser=None, enable_draw=False, pybullet_client=None):
+  def __init__(self, arg_parser=None, enable_draw=False, pybullet_client=None,
+               time_step=1./240,
+               init_strategy=InitializationStrategy.RANDOM):
     super().__init__(arg_parser, enable_draw)
     self._num_agents = 1
     self._pybullet_client = pybullet_client
     self._isInitialized = False
     self._useStablePD = True
     self._arg_parser = arg_parser
+    self.timeStep = time_step
+    self._init_strategy = init_strategy
+    print("Initialization strategy: {:s}".format(init_strategy))
     self.reset()
 
   def reset(self):
@@ -52,7 +64,7 @@ class PyBulletDeepMimicEnv(Env):
       motionPath = pybullet_data.getDataPath() + "/" + motion_file[0]
       #motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
       self._mocapData.Load(motionPath)
-      timeStep = 1. / 240.
+      timeStep = self.timeStep
       useFixedBase = False
       self._humanoid = humanoid_stable_pd.HumanoidStablePD(self._pybullet_client, self._mocapData,
                                                            timeStep, useFixedBase, self._arg_parser)
@@ -77,9 +89,14 @@ class PyBulletDeepMimicEnv(Env):
           time.sleep(timeStep)
     #print("numframes = ", self._humanoid._mocap_data.NumFrames())
     #startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)
-    rnrange = 1000
-    rn = random.randint(0, rnrange)
-    startTime = float(rn) / rnrange * self._humanoid.getCycleTime()
+    
+    if self._init_strategy == InitializationStrategy.RANDOM:
+      rnrange = 1000
+      rn = random.randint(0, rnrange)
+      startTime = float(rn) / rnrange * self._humanoid.getCycleTime()
+    elif self._init_strategy == InitializationStrategy.START:
+      startTime = 0
+    
     self.t = startTime
 
     self._humanoid.setSimTime(startTime)
@@ -263,6 +280,7 @@ class PyBulletDeepMimicEnv(Env):
     #print("pybullet_deep_mimic_env:update timeStep=",timeStep," t=",self.t)
     self._pybullet_client.setTimeStep(timeStep)
     self._humanoid._timeStep = timeStep
+    self.timeStep = timeStep
 
     for i in range(1):
       self.t += timeStep
@@ -314,7 +332,7 @@ class PyBulletDeepMimicEnv(Env):
 
   def check_valid_episode(self):
     #could check if limbs exceed velocity threshold
-    return true
+    return True
 
   def getKeyboardEvents(self):
     return self._pybullet_client.getKeyboardEvents()

examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py

@@ -19,18 +19,29 @@ import pybullet as p2
 import pybullet_data
 from pybullet_utils import bullet_client as bc
 from pkg_resources import parse_version
-from pybullet_envs.deep_mimic.env.pybullet_deep_mimic_env import PyBulletDeepMimicEnv
+from pybullet_envs.deep_mimic.env.pybullet_deep_mimic_env import PyBulletDeepMimicEnv, InitializationStrategy
 from pybullet_utils.arg_parser import ArgParser
 from pybullet_utils.logger import Logger
 
+from typing import Optional
+
 logger = logging.getLogger(__name__)
 
 
 class HumanoidDeepBulletEnv(gym.Env):
+  """Base Gym environment for DeepMimic."""
   metadata = {'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 50}
 
-  def __init__(self, renders=False, arg_file=''):
-    
+  def __init__(self, renders=False, arg_file='', test_mode=False,
+               time_step=1./240,
+               rescale_actions=True,
+               rescale_observations=True):
+    """    
+    Args:
+      test_mode (bool): in test mode, the `reset()` method will always set the mocap clip time
+      to 0.
+      time_step (float): physics time step.
+    """
     self._arg_parser = ArgParser()
     Logger.print2("===========================================================")
     succ = False
@@ -39,15 +50,33 @@ class HumanoidDeepBulletEnv(gym.Env):
       succ = self._arg_parser.load_file(path)
       Logger.print2(arg_file)
     assert succ, Logger.print2('Failed to load args from: ' + arg_file)
-    
-    self._p = None
-    self._time_step = 1./240.
-    self._internal_env =  None
+
+    self._p: Optional[BulletClient] = None
+    self._time_step = time_step
+    self._internal_env: Optional[PyBulletDeepMimicEnv] = None
     self._renders = renders
     self._discrete_actions = False
-    self._arg_file=arg_file
-    self._render_height = 200
-    self._render_width = 320
+    self._arg_file = arg_file
+    self._render_height = 400
+    self._render_width = 640
+    self._rescale_actions = rescale_actions
+    self._rescale_observations = rescale_observations
+    self.agent_id = -1
+
+    self._numSteps = None
+    self.test_mode = test_mode
+    if self.test_mode:
+        print("Environment running in TEST mode")
+
+    self.reset()
+
+    # Query the policy at 30Hz
+    self.policy_query_30 = True
+    if self.policy_query_30:
+        self._policy_step = 1./30
+    else:
+        self._policy_step = 1./240
+    self._num_env_steps = int(self._policy_step / self._time_step)
     
     self.theta_threshold_radians = 12 * 2 * math.pi / 360
     self.x_threshold = 0.4  #2.4
@@ -94,8 +123,6 @@ class HumanoidDeepBulletEnv(gym.Env):
     self.viewer = None
     self._configure()
     
-    # Query the policy at 30Hz
-    self.policy_query_30 = True
 
   def _configure(self, display=None):
     self.display = display
@@ -105,42 +132,73 @@ class HumanoidDeepBulletEnv(gym.Env):
     return [seed]
 
   def step(self, action):
-    
-    #apply control action
-    agent_id = -1
+    agent_id = self.agent_id
+
+    if self._rescale_actions:
+      # Rescale the action
+      mean = -self._action_offset
+      std = 1./self._action_scale
+      action = action * std + mean
+
+    # Record reward
+    reward = self._internal_env.calc_reward(agent_id)
+
+    # Apply control action
     self._internal_env.set_action(agent_id, action)
-    
-    #step sim
-    self._internal_env.update(self._time_step)
-    
-    if self.policy_query_30:
-      # Step simulation until a new action is required
-      while not self._internal_env.need_new_action(agent_id):
-        self._p.stepSimulation()
-        self._internal_env.t += self._time_step
-          # print("t:", self._internal_env.t)
-    # print("next update time:", self._internal_env.needs_update_time)
-    # print("Performing update")
-        
-    #record state
+
+    start_time = self._internal_env.t
+
+    # step sim
+    for i in range(self._num_env_steps):
+        self._internal_env.update(self._time_step)
+
+    elapsed_time = self._internal_env.t - start_time
+
+    self._numSteps += 1
+
+    # Record state
     self.state = self._internal_env.record_state(agent_id)
     
-    #record reward
-    reward = self._internal_env.calc_reward(agent_id)
-    
-    #record done
+    if self._rescale_observations:
+      state = np.array(self.state)
+      mean = -self._state_offset
+      std = 1./self._state_scale
+      state = (state - mean) / (std + 1e-8)
+
+    # Record done
     done = self._internal_env.is_episode_end()
     
-    return np.array(self.state), reward, done, {}
+    info = {}
+    return state, reward, done, info
 
   def reset(self):
-    #    print("-----------reset simulation---------------")
-    if self._internal_env==None:
-      self._internal_env = PyBulletDeepMimicEnv(self._arg_parser, self._renders)
+    # use the initialization strategy
+    if self._internal_env is None:
+      if self.test_mode:
+        init_strat = InitializationStrategy.START
+      else:
+        init_strat = InitializationStrategy.RANDOM
+      self._internal_env = PyBulletDeepMimicEnv(self._arg_parser, self._renders,
+                                                time_step=self._time_step,
+                                                init_strategy=init_strat)
+
     self._internal_env.reset()
     self._p = self._internal_env._pybullet_client
-    agent_id = -1 #unused here
-    state = self._internal_env.record_state(agent_id)
+    agent_id = self.agent_id  # unused here
+    self._state_offset = self._internal_env.build_state_offset(self.agent_id)
+    self._state_scale = self._internal_env.build_state_scale(self.agent_id)
+    self._action_offset = self._internal_env.build_action_offset(self.agent_id)
+    self._action_scale = self._internal_env.build_action_scale(self.agent_id)
+    self._numSteps = 0
+    # Record state
+    self.state = self._internal_env.record_state(agent_id)
+
+    # return state as ndarray
+    state = np.array(self.state)
+    if self._rescale_observations:
+      mean = -self._state_offset
+      std = 1./self._state_scale
+      state = (state - mean) / (std + 1e-8)
     return state
 
   def render(self, mode='human', close=False):
@@ -148,8 +206,15 @@ class HumanoidDeepBulletEnv(gym.Env):
       self._renders = True
     if mode != "rgb_array":
       return np.array([])
-    base_pos=[0,0,0]
-    self._cam_dist = 2
+    human = self._internal_env._humanoid
+    base_pos, orn = self._p.getBasePositionAndOrientation(human._sim_model)
+    base_pos = np.asarray(base_pos)
+    # track the position
+    base_pos[1] += 0.3
+    rpy = self._p.getEulerFromQuaternion(orn)  # rpy, in radians
+    rpy = 180 / np.pi * np.asarray(rpy)  # convert rpy in degrees
+
+    self._cam_dist = 3
     self._cam_pitch = 0.3
     self._cam_yaw = 0
     if (not self._p == None):
@@ -159,10 +224,9 @@ class HumanoidDeepBulletEnv(gym.Env):
         yaw=self._cam_yaw,
         pitch=self._cam_pitch,
         roll=0,
-        upAxisIndex=2)
+        upAxisIndex=1)
       proj_matrix = self._p.computeProjectionMatrixFOV(fov=60,
-             aspect=float(self._render_width) /
-             self._render_height,
+             aspect=float(self._render_width) / self._render_height,
              nearVal=0.1,
              farVal=100.0)
       (_, _, px, _, _) = self._p.getCameraImage(
@@ -171,6 +235,12 @@ class HumanoidDeepBulletEnv(gym.Env):
           renderer=self._p.ER_BULLET_HARDWARE_OPENGL,
           viewMatrix=view_matrix,
           projectionMatrix=proj_matrix)
+      # self._p.resetDebugVisualizerCamera(
+      #   cameraDistance=2 * self._cam_dist,
+      #   cameraYaw=self._cam_yaw,
+      #   cameraPitch=self._cam_pitch,
+      #   cameraTargetPosition=base_pos
+      # )
     else:
       px = np.array([[[255,255,255,255]]*self._render_width]*self._render_height, dtype=np.uint8)
     rgb_array = np.array(px, dtype=np.uint8)

src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -48,6 +48,7 @@ struct btContactSolverInfoData
 	btScalar m_erp2;         //error reduction for contact constraints
 	btScalar m_deformable_erp;          //error reduction for deformable constraints
 	btScalar m_deformable_cfm;          //constraint force mixing for deformable constraints
+	btScalar m_deformable_maxErrorReduction; // maxErrorReduction for deformable contact
 	btScalar m_globalCfm;    //constraint force mixing for contacts and non-contacts
 	btScalar m_frictionERP;  //error reduction for friction constraints
 	btScalar m_frictionCFM;  //constraint force mixing for friction constraints
@@ -86,6 +87,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_erp2 = btScalar(0.2);
 		m_deformable_erp = btScalar(0.06);
 		m_deformable_cfm = btScalar(0.01);
+		m_deformable_maxErrorReduction = btScalar(0.1);
 		m_globalCfm = btScalar(0.);
 		m_frictionERP = btScalar(0.2);  //positional friction 'anchors' are disabled by default
 		m_frictionCFM = btScalar(0.);

src/BulletSoftBody/btConjugateGradient.h

@@ -37,6 +37,12 @@ public:
 		BT_PROFILE("CGSolve");
 		btAssert(x.size() == b.size());
 		reinitialize(b);
+		temp = b;
+		A.project(temp);
+		p = temp;
+		A.precondition(p, z);
+		btScalar d0 = this->dot(z, temp);
+		d0 = btMin(btScalar(1), d0);
 		// r = b - A * x --with assigned dof zeroed out
 		A.multiply(x, temp);
 		r = this->sub(b, temp);
@@ -45,7 +51,7 @@ public:
 		A.precondition(r, z);
 		A.project(z);
 		btScalar r_dot_z = this->dot(z, r);
-		if (r_dot_z <= Base::m_tolerance)
+		if (r_dot_z <= Base::m_tolerance * d0)
 		{
 			if (verbose)
 			{
@@ -61,7 +67,7 @@ public:
 			// temp = A*p
 			A.multiply(p, temp);
 			A.project(temp);
-			if (this->dot(p, temp) < SIMD_EPSILON)
+			if (this->dot(p, temp) < 0)
 			{
 				if (verbose)
 					std::cout << "Encountered negative direction in CG!" << std::endl;
@@ -81,11 +87,11 @@ public:
 			A.precondition(r, z);
 			r_dot_z = r_dot_z_new;
 			r_dot_z_new = this->dot(r, z);
-			if (r_dot_z_new < Base::m_tolerance)
+			if (r_dot_z_new < Base::m_tolerance * d0)
 			{
 				if (verbose)
 				{
-					std::cout << "ConjugateGradient iterations " << k << std::endl;
+					std::cout << "ConjugateGradient iterations " << k << " residual = " << r_dot_z_new << std::endl;
 				}
 				return k;
 			}
@@ -95,7 +101,7 @@ public:
 		}
 		if (verbose)
 		{
-			std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << std::endl;
+			std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << " error = " << r_dot_z_new << std::endl;
 		}
 		return Base::m_maxIterations;
 	}

src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp

@@ -46,6 +46,17 @@ void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar
 	{
 		m_lf[i]->reinitialize(nodeUpdated);
 	}
+	btMatrix3x3 I;
+	I.setIdentity();
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			if (psb->m_nodes[j].m_im > 0)
+				psb->m_nodes[j].m_effectiveMass = I * (1.0 / psb->m_nodes[j].m_im);
+		}
+	}
 	m_projection.reinitialize(nodeUpdated);
 	//    m_preconditioner->reinitialize(nodeUpdated);
 }
@@ -133,10 +144,23 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero
 			counter += psb->m_nodes.size();
 			continue;
 		}
-		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		if (m_implicit)
 		{
-			btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im;
-			psb->m_nodes[j].m_v += one_over_mass * force[counter++];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				if (psb->m_nodes[j].m_im != 0)
+				{
+					psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++];
+				}
+			}
+		}
+		else
+		{
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im;
+				psb->m_nodes[j].m_v += one_over_mass * force[counter++];
+			}
 		}
 	}
 	if (setZero)
@@ -190,10 +214,56 @@ void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
 	{
 		m_softBodies[i]->advanceDeformation();
 	}
-
-	for (int i = 0; i < m_lf.size(); ++i)
+	if (m_implicit)
 	{
-		m_lf[i]->addScaledExplicitForce(m_dt, force);
+		// apply forces except gravity force
+		btVector3 gravity;
+		for (int i = 0; i < m_lf.size(); ++i)
+		{
+			if (m_lf[i]->getForceType() == BT_GRAVITY_FORCE)
+			{
+				gravity = static_cast<btDeformableGravityForce*>(m_lf[i])->m_gravity;
+			}
+			else
+			{
+				m_lf[i]->addScaledForces(m_dt, force);
+			}
+		}
+		for (int i = 0; i < m_lf.size(); ++i)
+		{
+			m_lf[i]->addScaledHessian(m_dt);
+		}
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					// add gravity explicitly
+					psb->m_nodes[j].m_v += m_dt * psb->m_gravityFactor * gravity;
+				}
+			}
+		}
+	}
+	else
+	{
+		for (int i = 0; i < m_lf.size(); ++i)
+		{
+			m_lf[i]->addScaledExplicitForce(m_dt, force);
+		}
+	}
+	// calculate inverse mass matrix for all nodes
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (psb->isActive())
+		{
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				psb->m_nodes[j].m_effectiveMass_inv = psb->m_nodes[j].m_effectiveMass.inverse();
+			}
+		}
 	}
 	applyForce(force, true);
 }

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -20,7 +20,7 @@
 #include "LinearMath/btQuickprof.h"
 static const int kMaxConjugateGradientIterations = 300;
 btDeformableBodySolver::btDeformableBodySolver()
-	: m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(5), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false)
+	: m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false)
 {
 	m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
 }
@@ -276,7 +276,14 @@ void btDeformableBodySolver::updateVelocity()
 			{
 				m_dv[counter].setZero();
 			}
-			psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv;
+			if (m_implicit)
+			{
+				psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter];
+			}
+			else
+			{
+				psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv;
+			}
 			psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
 			++counter;
 		}
@@ -296,7 +303,7 @@ void btDeformableBodySolver::updateTempPosition()
 		}
 		for (int j = 0; j < psb->m_nodes.size(); ++j)
 		{
-			psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v;
+			psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * (psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv);
 			++counter;
 		}
 		psb->updateDeformation();
@@ -331,8 +338,9 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit)
 		{
 			if (implicit)
 			{
-				if ((psb->m_nodes[j].m_v - m_backupVelocity[counter]).norm() < SIMD_EPSILON)
-					m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
+				// setting the initial guess for newton, need m_dv = v_{n+1} - v_n for dofs that are in constraint.
+				if (psb->m_nodes[j].m_v == m_backupVelocity[counter])
+					m_dv[counter].setZero();
 				else
 					m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn;
 				m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
@@ -376,6 +384,20 @@ bool btDeformableBodySolver::updateNodes()
 void btDeformableBodySolver::predictMotion(btScalar solverdt)
 {
 	// apply explicit forces to velocity
+	if (m_implicit)
+	{
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + psb->m_nodes[j].m_v * solverdt;
+				}
+			}
+		}
+	}
 	m_objective->applyExplicitForce(m_residual);
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
@@ -424,12 +446,19 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
 		// apply drag
 		n.m_v *= (1 - psb->m_cfg.drag);
 		// scale velocity back
-		if (n.m_v.norm() > max_v)
+		if (m_implicit)
 		{
-			n.m_v.safeNormalize();
-			n.m_v *= max_v;
+			n.m_q = n.m_x;
+		}
+		else
+		{
+			if (n.m_v.norm() > max_v)
+			{
+				n.m_v.safeNormalize();
+				n.m_v *= max_v;
+			}
+			n.m_q = n.m_x + n.m_v * dt;
 		}
-		n.m_q = n.m_x + n.m_v * dt;
 		n.m_splitv.setZero();
 		n.m_constrained = false;
 	}

src/BulletSoftBody/btDeformableContactConstraint.cpp

@@ -278,19 +278,19 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
 	btVector3 impulse_tangent = impulse - impulse_normal;
 	if (dn > 0)
 	{
-		m_binding = false;
 		return 0;
 	}
 	m_binding = true;
 	btScalar residualSquare = dn * dn;
 	btVector3 old_total_tangent_dv = m_total_tangent_dv;
-	// m_c2 is the inverse mass of the deformable node/face
-	m_total_normal_dv -= impulse_normal * m_contact->m_c2;
-	m_total_tangent_dv -= impulse_tangent * m_contact->m_c2;
+	// m_c5 is the inverse mass of the deformable node/face
+	m_total_normal_dv -= m_contact->m_c5 * impulse_normal;
+	m_total_tangent_dv -= m_contact->m_c5 * impulse_tangent;
 
 	if (m_total_normal_dv.dot(cti.m_normal) < 0)
 	{
 		// separating in the normal direction
+		m_binding = false;
 		m_static = false;
 		impulse_tangent.setZero();
 	}
@@ -309,7 +309,8 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
 			{
 				m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3;
 			}
-			impulse_tangent = -btScalar(1) / m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
+			//            impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
+			impulse_tangent = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv);
 		}
 		else
 		{
@@ -354,7 +355,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
 
 btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
 {
-	btScalar MAX_PENETRATION_CORRECTION = 0.1;
+	btScalar MAX_PENETRATION_CORRECTION = infoGlobal.m_deformable_maxErrorReduction;
 	const btSoftBody::sCti& cti = m_contact->m_cti;
 	btVector3 vb = getSplitVb();
 	btVector3 va = getSplitVa();
@@ -435,14 +436,14 @@ btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node*
 void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
 {
 	const btSoftBody::DeformableNodeRigidContact* contact = getContact();
-	btVector3 dv = impulse * contact->m_c2;
+	btVector3 dv = contact->m_c5 * impulse;
 	contact->m_node->m_v -= dv;
 }
 
 void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
 {
 	const btSoftBody::DeformableNodeRigidContact* contact = getContact();
-	btVector3 dv = impulse * contact->m_c2;
+	btVector3 dv = contact->m_c5 * impulse;
 	contact->m_node->m_splitv -= dv;
 }
 

src/BulletSoftBody/btDeformableContactProjection.cpp

@@ -70,6 +70,12 @@ btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** de
 			{
 				continue;
 			}
+			for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
+			{
+				btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
+				residualSquare = btMax(residualSquare, localResidualSquare);
+			}
 			for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
 			{
 				btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
@@ -140,15 +146,6 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
 			}
 			btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
 			m_faceRigidConstraints[i].push_back(constraint);
-			//            btVector3 va = constraint.getVa();
-			//            btVector3 vb = constraint.getVb();
-			//            const btVector3 vr = vb - va;
-			//            const btSoftBody::sCti& cti = contact.m_cti;
-			//            const btScalar dn = btDot(vr, cti.m_normal);
-			//            if (dn < SIMD_EPSILON)
-			//            {
-			//                m_faceRigidConstraints[i].push_back(constraint);
-			//            }
 		}
 	}
 }
@@ -175,7 +172,6 @@ void btDeformableContactProjection::project(TVStack& x)
 			if (free_dir.safeNorm() < SIMD_EPSILON)
 			{
 				x[i] -= x[i].dot(dir0) * dir0;
-				x[i] -= x[i].dot(dir1) * dir1;
 			}
 			else
 			{
@@ -256,33 +252,36 @@ void btDeformableContactProjection::setProjection()
 		{
 			int index = m_nodeRigidConstraints[i][j].m_node->index;
 			m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
-			if (m_nodeRigidConstraints[i][j].m_static)
+			if (m_nodeRigidConstraints[i][j].m_binding)
 			{
-				if (m_projectionsDict.find(index) == NULL)
+				if (m_nodeRigidConstraints[i][j].m_static)
 				{
-					m_projectionsDict.insert(index, units);
-				}
-				else
-				{
-					btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-					for (int k = 0; k < 3; ++k)
+					if (m_projectionsDict.find(index) == NULL)
 					{
-						projections.push_back(units[k]);
+						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);
+					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);
+					}
 				}
 			}
 		}
@@ -309,9 +308,9 @@ void btDeformableContactProjection::setProjection()
 					else
 					{
 						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-						for (int k = 0; k < 3; ++k)
+						for (int l = 0; l < 3; ++l)
 						{
-							projections.push_back(units[k]);
+							projections.push_back(units[l]);
 						}
 					}
 				}

src/BulletSoftBody/btDeformableLagrangianForce.h

@@ -66,6 +66,8 @@ public:
 	// add all damping forces
 	virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
 
+	virtual void addScaledHessian(btScalar scale) {}
+
 	virtual btDeformableLagrangianForceType getForceType() = 0;
 
 	virtual void reinitialize(bool nodeUpdated)

src/BulletSoftBody/btDeformableLinearElasticityForce.h

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

src/BulletSoftBody/btDeformableMousePickingForce.h

@@ -128,7 +128,24 @@ public:
 
 	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
 	{
-		//TODO
+		btScalar scaled_stiffness = scale * m_elasticStiffness;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
+			btScalar dir_norm = dir.norm();
+			btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
+			int id = m_face.m_n[i]->index;
+			btVector3 dx_diff = dx[id];
+			btScalar r = 0;  // rest length is 0 for picking spring
+			btVector3 scaled_df = btVector3(0, 0, 0);
+			if (dir_norm > SIMD_EPSILON)
+			{
+				scaled_df -= scaled_stiffness * dir_normalized.dot(dx_diff) * dir_normalized;
+				scaled_df += scaled_stiffness * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized;
+				scaled_df -= scaled_stiffness * ((dir_norm - r) / dir_norm) * dx_diff;
+			}
+			df[id] += scaled_df;
+		}
 	}
 
 	void setMousePos(const btVector3& p)

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

@@ -162,6 +162,11 @@ public:
 		m_lineSearch = lineSearch;
 	}
 
+	void setUseProjection(bool useProjection)
+	{
+		m_useProjection = useProjection;
+	}
+
 	void applyRepulsionForce(btScalar timeStep);
 
 	void performGeometricCollisions(btScalar timeStep);

src/BulletSoftBody/btSoftBody.cpp

@@ -900,6 +900,7 @@ void btSoftBody::setVelocity(const btVector3& velocity)
 		if (n.m_im > 0)
 		{
 			n.m_v = velocity;
+			n.m_vn = velocity;
 		}
 	}
 }
@@ -2772,6 +2773,7 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
 			shp,
 			nrm,
 			margin);
+
 	if (!predict)
 	{
 		cti.m_colObj = colObjWrap->getCollisionObject();
@@ -3462,11 +3464,46 @@ void btSoftBody::initializeDmInverse()
 					   c1.getZ(), c2.getZ(), c3.getZ());
 		t.m_element_measure = Dm.determinant() * unit_simplex_measure;
 		t.m_Dm_inverse = Dm.inverse();
+
+		// calculate the first three columns of P^{-1}
+		btVector3 a = t.m_n[0]->m_x;
+		btVector3 b = t.m_n[1]->m_x;
+		btVector3 c = t.m_n[2]->m_x;
+		btVector3 d = t.m_n[3]->m_x;
+
+		btScalar det = 1 / (a[0] * b[1] * c[2] - a[0] * b[1] * d[2] - a[0] * b[2] * c[1] + a[0] * b[2] * d[1] + a[0] * c[1] * d[2] - a[0] * c[2] * d[1] + a[1] * (-b[0] * c[2] + b[0] * d[2] + b[2] * c[0] - b[2] * d[0] - c[0] * d[2] + c[2] * d[0]) + a[2] * (b[0] * c[1] - b[0] * d[1] + b[1] * (d[0] - c[0]) + c[0] * d[1] - c[1] * d[0]) - b[0] * c[1] * d[2] + b[0] * c[2] * d[1] + b[1] * c[0] * d[2] - b[1] * c[2] * d[0] - b[2] * c[0] * d[1] + b[2] * c[1] * d[0]);
+
+		btScalar P11 = -b[2] * c[1] + d[2] * c[1] + b[1] * c[2] + b[2] * d[1] - c[2] * d[1] - b[1] * d[2];
+		btScalar P12 = b[2] * c[0] - d[2] * c[0] - b[0] * c[2] - b[2] * d[0] + c[2] * d[0] + b[0] * d[2];
+		btScalar P13 = -b[1] * c[0] + d[1] * c[0] + b[0] * c[1] + b[1] * d[0] - c[1] * d[0] - b[0] * d[1];
+		btScalar P21 = a[2] * c[1] - d[2] * c[1] - a[1] * c[2] - a[2] * d[1] + c[2] * d[1] + a[1] * d[2];
+		btScalar P22 = -a[2] * c[0] + d[2] * c[0] + a[0] * c[2] + a[2] * d[0] - c[2] * d[0] - a[0] * d[2];
+		btScalar P23 = a[1] * c[0] - d[1] * c[0] - a[0] * c[1] - a[1] * d[0] + c[1] * d[0] + a[0] * d[1];
+		btScalar P31 = -a[2] * b[1] + d[2] * b[1] + a[1] * b[2] + a[2] * d[1] - b[2] * d[1] - a[1] * d[2];
+		btScalar P32 = a[2] * b[0] - d[2] * b[0] - a[0] * b[2] - a[2] * d[0] + b[2] * d[0] + a[0] * d[2];
+		btScalar P33 = -a[1] * b[0] + d[1] * b[0] + a[0] * b[1] + a[1] * d[0] - b[1] * d[0] - a[0] * d[1];
+		btScalar P41 = a[2] * b[1] - c[2] * b[1] - a[1] * b[2] - a[2] * c[1] + b[2] * c[1] + a[1] * c[2];
+		btScalar P42 = -a[2] * b[0] + c[2] * b[0] + a[0] * b[2] + a[2] * c[0] - b[2] * c[0] - a[0] * c[2];
+		btScalar P43 = a[1] * b[0] - c[1] * b[0] - a[0] * b[1] - a[1] * c[0] + b[1] * c[0] + a[0] * c[1];
+
+		btVector4 p1(P11 * det, P21 * det, P31 * det, P41 * det);
+		btVector4 p2(P12 * det, P22 * det, P32 * det, P42 * det);
+		btVector4 p3(P13 * det, P23 * det, P33 * det, P43 * det);
+
+		t.m_P_inv[0] = p1;
+		t.m_P_inv[1] = p2;
+		t.m_P_inv[2] = p3;
 	}
 }
 
+static btScalar Dot4(const btVector4& a, const btVector4& b)
+{
+	return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
+}
+
 void btSoftBody::updateDeformation()
 {
+	btQuaternion q;
 	for (int i = 0; i < m_tetras.size(); ++i)
 	{
 		btSoftBody::Tetra& t = m_tetras[i];
@@ -3484,6 +3521,21 @@ void btSoftBody::updateDeformation()
 		btMatrix3x3 C = t.m_F.transpose() * t.m_F;
 		s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ();
 		s.m_cofF = t.m_F.adjoint().transpose();
+
+		btVector3 a = t.m_n[0]->m_q;
+		btVector3 b = t.m_n[1]->m_q;
+		btVector3 c = t.m_n[2]->m_q;
+		btVector3 d = t.m_n[3]->m_q;
+		btVector4 q1(a[0], b[0], c[0], d[0]);
+		btVector4 q2(a[1], b[1], c[1], d[1]);
+		btVector4 q3(a[2], b[2], c[2], d[2]);
+		btMatrix3x3 B(Dot4(q1, t.m_P_inv[0]), Dot4(q1, t.m_P_inv[1]), Dot4(q1, t.m_P_inv[2]),
+					  Dot4(q2, t.m_P_inv[0]), Dot4(q2, t.m_P_inv[1]), Dot4(q2, t.m_P_inv[2]),
+					  Dot4(q3, t.m_P_inv[0]), Dot4(q3, t.m_P_inv[1]), Dot4(q3, t.m_P_inv[2]));
+		q.setRotation(btVector3(0, 0, 1), 0);
+		B.extractRotation(q, 0.01);  // precision of the rotation is not very important for visual correctness.
+		btMatrix3x3 Q(q);
+		s.m_corotation = Q;
 	}
 }
 

src/BulletSoftBody/btSoftBody.h

@@ -272,7 +272,9 @@ public:
 		int m_constrained;   // depth of penetration
 		int m_battach : 1;   // Attached
 		int index;
-		btVector3 m_splitv;  // velocity associated with split impulse
+		btVector3 m_splitv;               // velocity associated with split impulse
+		btMatrix3x3 m_effectiveMass;      // effective mass in contact
+		btMatrix3x3 m_effectiveMass_inv;  // inverse of effective mass
 	};
 	/* Link			*/
 	ATTRIBUTE_ALIGNED16(struct)
@@ -311,15 +313,17 @@ public:
 		btMatrix3x3 m_Dm_inverse;  // rest Dm^-1
 		btMatrix3x3 m_F;
 		btScalar m_element_measure;
+		btVector4 m_P_inv[3];  // first three columns of P_inv matrix
 	};
 
 	/*  TetraScratch  */
 	struct TetraScratch
 	{
-		btMatrix3x3 m_F;     // deformation gradient F
-		btScalar m_trace;    // trace of F^T * F
-		btScalar m_J;        // det(F)
-		btMatrix3x3 m_cofF;  // cofactor of F
+		btMatrix3x3 m_F;           // deformation gradient F
+		btScalar m_trace;          // trace of F^T * F
+		btScalar m_J;              // det(F)
+		btMatrix3x3 m_cofF;        // cofactor of F
+		btMatrix3x3 m_corotation;  // corotatio of the tetra
 	};
 
 	/* RContact		*/
@@ -350,6 +354,7 @@ public:
 		btScalar m_c2;     // inverse mass of node/face
 		btScalar m_c3;     // Friction
 		btScalar m_c4;     // Hardness
+		btMatrix3x3 m_c5;  // inverse effective mass
 
 		// jacobians and unit impulse responses for multibody
 		btMultiBodyJacobianData jacobianData_normal;
@@ -1323,7 +1328,7 @@ public:
 			int face_penetration = 0, node_penetration = node->m_constrained;
 			for (int i = 0; i < 3; ++i)
 				face_penetration |= face->m_n[i]->m_constrained;
-			btScalar I_tilde = .5 * I / (1.0 + w.length2());
+			btScalar I_tilde = 2.0 * I / (1.0 + w.length2());
 
 			//             double the impulse if node or face is constrained.
 			if (face_penetration > 0 || node_penetration > 0)
@@ -1349,10 +1354,10 @@ public:
 				btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0)) * vt_norm;
 				I = 0.5 * mass * (vt_norm - vt_new);
 				vt.safeNormalize();
-				I_tilde = .5 * I / (1.0 + w.length2());
+				I_tilde = 2.0 * I / (1.0 + w.length2());
 				//                 double the impulse if node or face is constrained.
-				//                if (face_penetration > 0 || node_penetration > 0)
-				//                    I_tilde *= 2.0;
+				if (face_penetration > 0 || node_penetration > 0)
+					I_tilde *= 2.0;
 				if (face_penetration <= 0)
 				{
 					for (int j = 0; j < 3; ++j)

src/BulletSoftBody/btSoftBodyInternals.h

@@ -1010,6 +1010,20 @@ static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
 	return (Diagonal(1 / dt) * Add(Diagonal(ima), MassMatrix(imb, iwi, r)).inverse());
 }
 
+//
+static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
+										const btMatrix3x3& effective_mass_inv,
+										btScalar imb,
+										const btMatrix3x3& iwi,
+										const btVector3& r)
+{
+	return (Diagonal(1 / dt) * Add(effective_mass_inv, MassMatrix(imb, iwi, r)).inverse());
+	//    btMatrix3x3 iimb = MassMatrix(imb, iwi, r);
+	//    if (iimb.determinant() == 0)
+	//        return effective_mass_inv.inverse();
+	//    return effective_mass_inv.inverse() *  Add(effective_mass_inv.inverse(), iimb.inverse()).inverse() * iimb.inverse();
+}
+
 //
 static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra,
 										btScalar imb, const btMatrix3x3& iib, const btVector3& rb)
@@ -1132,9 +1146,7 @@ static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVec
 	if (dir_norm < SIMD_EPSILON)
 		return false;
 	dir.normalize();
-
 	btScalar t;
-
 	bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t);
 
 	if (ret)
@@ -1682,7 +1694,9 @@ struct btSoftColliders
 							const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
 							const btVector3 ra = n.m_x - wtr.getOrigin();
 
-							c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+							c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
+							c.m_c5 = n.m_effectiveMass_inv;
+							//                            c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
 							c.m_c1 = ra;
 						}
 						else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
@@ -1710,7 +1724,7 @@ struct btSoftColliders
 												t1.getX(), t1.getY(), t1.getZ(),
 												t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
 								const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-								btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+								btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
 								c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
 								c.jacobianData_normal = jacobianData_normal;
 								c.jacobianData_t1 = jacobianData_t1;
@@ -1776,6 +1790,7 @@ struct btSoftColliders
 					c.m_c2 = ima;
 					c.m_c3 = fc;
 					c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+					c.m_c5 = Diagonal(ima);
 					if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 					{
 						const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();