Merge pull request #2373 from xhan0619/DeformableImprovement

Deformable improvement

examples/DeformableDemo/DeformableMultibody.cpp

@@ -28,7 +28,7 @@
 #include "DeformableMultibody.h"
 ///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
 #include "btBulletDynamicsCommon.h"
-#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
 #include "BulletSoftBody/btSoftBody.h"
 #include "BulletSoftBody/btSoftBodyHelpers.h"
 #include "BulletSoftBody/btDeformableBodySolver.h"
@@ -49,8 +49,7 @@ static bool g_floatingBase = true;
 static float friction = 1.;
 class DeformableMultibody : public CommonMultiBodyBase
 {
-    btMultiBody* m_multiBody;
+    btAlignedObjectArray<btDeformableLagrangianForce*> forces;
-    btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
 public:
 	DeformableMultibody(struct GUIHelperInterface* helper)
 		: CommonMultiBodyBase(helper)
@@ -81,20 +80,20 @@ public:
     void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
     
 
-    virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
+    virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
     {
-        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
     }
     
-    virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld()
+    virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
     {
-        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
     }
     
     virtual void renderScene()
     {
         CommonMultiBodyBase::renderScene();
-        btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
         
         for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
         {
@@ -119,12 +118,12 @@ void DeformableMultibody::initPhysics()
 
 	m_broadphase = new btDbvtBroadphase();
     btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
-    btMultiBodyConstraintSolver* sol;
+    btDeformableMultiBodyConstraintSolver* sol;
-    sol = new btMultiBodyConstraintSolver;
+    sol = new btDeformableMultiBodyConstraintSolver;
+    sol->setDeformableSolver(deformableBodySolver);
 	m_solver = sol;
     
-    m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
+    m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
-    deformableBodySolver->setWorld(getDeformableDynamicsWorld());
     btVector3 gravity = btVector3(0, -10, 0);
 	m_dynamicsWorld->setGravity(gravity);
     getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
@@ -221,15 +220,20 @@ void DeformableMultibody::initPhysics()
         psb->getCollisionShape()->setMargin(0.25);
         psb->generateBendingConstraints(2);
         psb->setTotalMass(5);
-        psb->setSpringStiffness(2);
-        psb->setDampingCoefficient(0.01);
         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.collisions = btSoftBody::fCollision::SDF_RD;
+        psb->setCollisionFlags(0);
         getDeformableDynamicsWorld()->addSoftBody(psb);
-        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
+
-        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
+        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2, 0.01, false);
+        getDeformableDynamicsWorld()->addForce(psb, mass_spring);
+        forces.push_back(mass_spring);
+        
+        btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(gravity);
+        getDeformableDynamicsWorld()->addForce(psb, gravity_force);
+        forces.push_back(gravity_force);
     }
 
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
@@ -252,7 +256,12 @@ void DeformableMultibody::exitPhysics()
 		m_dynamicsWorld->removeCollisionObject(obj);
 		delete obj;
 	}
-
+    // delete forces
+    for (int j = 0; j < forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = forces[j];
+        delete force;
+    }
 	//delete collision shapes
 	for (int j = 0; j < m_collisionShapes.size(); j++)
 	{

examples/DeformableDemo/DeformableRigid.cpp

@@ -28,7 +28,7 @@
 #include "DeformableRigid.h"
 ///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
 #include "btBulletDynamicsCommon.h"
-#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
 #include "BulletSoftBody/btSoftBody.h"
 #include "BulletSoftBody/btSoftBodyHelpers.h"
 #include "BulletSoftBody/btDeformableBodySolver.h"
@@ -44,6 +44,7 @@
 ///Generally it is best to leave the rolling friction coefficient zero (or close to zero).
 class DeformableRigid : public CommonRigidBodyBase
 {
+    btAlignedObjectArray<btDeformableLagrangianForce*> forces;
 public:
 	DeformableRigid(struct GUIHelperInterface* helper)
 		: CommonRigidBodyBase(helper)
@@ -115,24 +116,24 @@ public:
         createRigidBody(mass, startTransform, shape[0]);
     }
     
-    virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
+    virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
     {
         ///just make it a btSoftRigidDynamicsWorld please
         ///or we will add type checking
-        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
     }
     
-    virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld()
+    virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
     {
         ///just make it a btSoftRigidDynamicsWorld please
         ///or we will add type checking
-        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
     }
     
     virtual void renderScene()
     {
         CommonRigidBodyBase::renderScene();
-        btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
         
         for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
         {
@@ -160,11 +161,12 @@ void DeformableRigid::initPhysics()
     btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
 
 	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
-	btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver();
+	btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(deformableBodySolver);
 	m_solver = sol;
 
-	m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
+	m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
-    deformableBodySolver->setWorld(getDeformableDynamicsWorld());
+//    deformableBodySolver->setWorld(getDeformableDynamicsWorld());
 	//	m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
     btVector3 gravity = btVector3(0, -10, 0);
 	m_dynamicsWorld->setGravity(gravity);
@@ -207,10 +209,12 @@ void DeformableRigid::initPhysics()
     {
         bool onGround = false;
         const btScalar s = 4;
-        btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
+        const btScalar h = 0;
-                                                         btVector3(+s, 0, -s),
+        
-                                                         btVector3(-s, 0, +s),
+        btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
-                                                         btVector3(+s, 0, +s),
+                                                         btVector3(+s, h, -s),
+                                                         btVector3(-s, h, +s),
+                                                         btVector3(+s, h, +s),
 //                                                         3,3,
                                                           20,20,
                                                           1 + 2 + 4 + 8, true);
@@ -228,16 +232,19 @@ void DeformableRigid::initPhysics()
         psb->getCollisionShape()->setMargin(0.1);
         psb->generateBendingConstraints(2);
         psb->setTotalMass(1);
-        psb->setSpringStiffness(1);
-        psb->setDampingCoefficient(0.01);
         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.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addSoftBody(psb);
-        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
-        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
         
+        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.01, false);
+        getDeformableDynamicsWorld()->addForce(psb, mass_spring);
+        forces.push_back(mass_spring);
+        
+        btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(gravity);
+        getDeformableDynamicsWorld()->addForce(psb, gravity_force);
+        forces.push_back(gravity_force);
         // add a few rigid bodies
         Ctor_RbUpStack(1);
     }
@@ -261,7 +268,12 @@ void DeformableRigid::exitPhysics()
 		m_dynamicsWorld->removeCollisionObject(obj);
 		delete obj;
 	}
-
+    // delete forces
+    for (int j = 0; j < forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = forces[j];
+        delete force;
+    }
 	//delete collision shapes
 	for (int j = 0; j < m_collisionShapes.size(); j++)
 	{

examples/DeformableDemo/GraspDeformable.cpp

@@ -0,0 +1,592 @@
+/*
+ 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 "GraspDeformable.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 "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
+#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
+#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "../CommonInterfaces/CommonFileIOInterface.h"
+#include "Bullet3Common/b3FileUtils.h"
+
+///The GraspDeformable shows the use of rolling friction.
+///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same.
+///Generally it is best to leave the rolling friction coefficient zero (or close to zero).
+static btScalar sGripperVerticalVelocity = 0.f;
+static btScalar sGripperClosingTargetVelocity = 0.f;
+static float friction = 1.;
+struct TetraCube
+{
+#include "../SoftDemo/cube.inl"
+};
+
+struct TetraBunny
+{
+#include "../SoftDemo/bunny.inl"
+};
+
+static bool supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
+{
+    bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute
+                         || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic);
+    return canHaveMotor;
+}
+
+class GraspDeformable : public CommonRigidBodyBase
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*> forces;
+public:
+	GraspDeformable(struct GUIHelperInterface* helper)
+		: CommonRigidBodyBase(helper)
+	{
+	}
+	virtual ~GraspDeformable()
+	{
+	}
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 2;
+        float pitch = -45;
+        float yaw = 100;
+        float targetPos[3] = {0, -0, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+    
+    btMultiBody* createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld,const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool floating);
+    
+    void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
+    
+    btMultiBody* createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating);
+    
+    void stepSimulation(float deltaTime)
+    {
+        double fingerTargetVelocities[2] = {sGripperVerticalVelocity, sGripperClosingTargetVelocity};
+        int num_multiBody = getDeformableDynamicsWorld()->getNumMultibodies();
+        for (int i = 0; i < num_multiBody; ++i)
+        {
+            btMultiBody* mb = getDeformableDynamicsWorld()->btMultiBodyDynamicsWorld::getMultiBody(i);
+            mb->setBaseVel(btVector3(0,sGripperVerticalVelocity, 0));
+            int dofIndex = 6;  //skip the 3 linear + 3 angular degree of freedom entries of the base
+            for (int link = 0; link < mb->getNumLinks(); link++)
+            {
+                if (supportsJointMotor(mb, link))
+                {
+                    btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
+                    if (motor)
+                    {
+//                        if (dofIndex == 10 || dofIndex == 11)
+//                        {
+//                            motor->setVelocityTarget(fingerTargetVelocities[1], 1);
+//                            motor->setMaxAppliedImpulse(1);
+//                        }
+                        if (dofIndex == 6)
+                        {
+                            motor->setVelocityTarget(-fingerTargetVelocities[1], 1);
+                            motor->setMaxAppliedImpulse(2);
+                        }
+                        if (dofIndex == 7)
+                        {
+                            motor->setVelocityTarget(fingerTargetVelocities[1], 1);
+                            motor->setMaxAppliedImpulse(2);
+                        }
+//                        motor->setRhsClamp(SIMD_INFINITY);
+                        motor->setMaxAppliedImpulse(1);
+                    }
+                }
+                dofIndex += mb->getLink(link).m_dofCount;
+            }
+        }
+        
+        //use a smaller internal timestep, there are stability issues
+        float internalTimeStep = 1. / 250.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 100, internalTimeStep);
+    }
+    
+    void createGrip()
+    {
+        int count = 2;
+        float mass = 2;
+        btCollisionShape* shape[] = {
+            new btBoxShape(btVector3(3, 3, 0.5)),
+        };
+        static const int nshapes = sizeof(shape) / sizeof(shape[0]);
+        for (int i = 0; i < count; ++i)
+        {
+            btTransform startTransform;
+            startTransform.setIdentity();
+            startTransform.setOrigin(btVector3(10, 0, 0));
+            startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
+            createRigidBody(mass, startTransform, shape[i % nshapes]);
+        }
+    }
+    
+    virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
+    {
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
+    {
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual void renderScene()
+    {
+        CommonRigidBodyBase::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 GraspDeformable::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+    
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+	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);
+//    deformableBodySolver->setWorld(getDeformableDynamicsWorld());
+    btVector3 gravity = btVector3(0, -9.81, 0);
+	m_dynamicsWorld->setGravity(gravity);
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+//    // load a gripper
+//    {
+//        btTransform rootTrans;
+//        rootTrans.setIdentity();
+//        BulletURDFImporter u2b(m_guiHelper,0,0,50,0);
+//        bool forceFixedBase = false;
+//        bool loadOk = u2b.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", forceFixedBase);
+//        if (loadOk)
+//        {
+//            for (int m = 0; m < u2b.getNumModels(); m++)
+//            {
+//                u2b.activateModel(m);
+//
+//                btMultiBody* mb = 0;
+//
+//                MyMultiBodyCreator creation(m_guiHelper);
+//
+//                u2b.getRootTransformInWorld(rootTrans);
+//                ConvertURDF2Bullet(u2b, creation, rootTrans, getDeformableDynamicsWorld(), true, u2b.getPathPrefix(), CUF_USE_SDF+CUF_RESERVED);
+//                mb = creation.getBulletMultiBody();
+//
+//                int numLinks = mb->getNumLinks();
+//                for (int i = 0; i < numLinks; i++)
+//                {
+//                    int mbLinkIndex = i;
+//                    float maxMotorImpulse = 1.f;
+//
+//                    if (supportsJointMotor(mb, mbLinkIndex))
+//                    {
+//                        int dof = 0;
+//                        btScalar desiredVelocity = 0.f;
+//                        btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, dof, desiredVelocity, maxMotorImpulse);
+//                        motor->setPositionTarget(0, 0);
+//                        motor->setVelocityTarget(0, 1);
+//                        mb->getLink(mbLinkIndex).m_userPtr = motor;
+//                        getDeformableDynamicsWorld()->addMultiBodyConstraint(motor);
+//                        motor->finalizeMultiDof();
+//                    }
+//                }
+//            }
+//        }
+//    }
+    // build a gripper
+    {
+        bool damping = true;
+        bool gyro = false;
+        bool canSleep = false;
+        bool selfCollide = true;
+        int numLinks = 2;
+        btVector3 linkHalfExtents(.1, .2, .04);
+        btVector3 baseHalfExtents(.1, 0.02, .2);
+        btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, .7f,0.f), linkHalfExtents, baseHalfExtents, false);
+        
+        mbC->setCanSleep(canSleep);
+        mbC->setHasSelfCollision(selfCollide);
+        mbC->setUseGyroTerm(gyro);
+
+        for (int i = 0; i < numLinks; i++)
+        {
+            int mbLinkIndex = i;
+            float maxMotorImpulse = 1.f;
+
+            if (supportsJointMotor(mbC, mbLinkIndex))
+            {
+                int dof = 0;
+                btScalar desiredVelocity = 0.f;
+                btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mbC, mbLinkIndex, dof, desiredVelocity, maxMotorImpulse);
+                motor->setPositionTarget(0, 0);
+                motor->setVelocityTarget(0, 1);
+                mbC->getLink(mbLinkIndex).m_userPtr = motor;
+                getDeformableDynamicsWorld()->addMultiBodyConstraint(motor);
+                motor->finalizeMultiDof();
+            }
+        }
+        
+        
+        if (!damping)
+        {
+            mbC->setLinearDamping(0.0f);
+            mbC->setAngularDamping(0.0f);
+        }
+        else
+        {
+            mbC->setLinearDamping(0.04f);
+            mbC->setAngularDamping(0.04f);
+        }
+        btScalar q0 = 0.f * SIMD_PI / 180.f;
+        if (numLinks > 0)
+            mbC->setJointPosMultiDof(0, &q0);
+        ///
+        addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents);
+
+    }
+    //create a ground
+    {
+        btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.)));
+
+        m_collisionShapes.push_back(groundShape);
+
+        btTransform groundTransform;
+        groundTransform.setIdentity();
+        groundTransform.setOrigin(btVector3(0, -25-.6, 0));
+        groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
+        //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
+        btScalar mass(0.);
+
+        //rigidbody is dynamic if and only if mass is non zero, otherwise static
+        bool isDynamic = (mass != 0.f);
+
+        btVector3 localInertia(0, 0, 0);
+        if (isDynamic)
+            groundShape->calculateLocalInertia(mass, localInertia);
+
+        //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+        btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
+        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
+        btRigidBody* body = new btRigidBody(rbInfo);
+        body->setFriction(0.1);
+
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body,1,1+2);
+    }
+
+    // create a soft block
+    {
+        char relative_path[1024];
+//        b3FileUtils::findFile("banana.vtk", relative_path, 1024);
+         b3FileUtils::findFile("ball.vtk", relative_path, 1024);
+//         b3FileUtils::findFile("tube.vtk", relative_path, 1024);
+//         b3FileUtils::findFile("torus.vtk", relative_path, 1024);
+//         b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024);
+//         b3FileUtils::findFile("bread.vtk", relative_path, 1024);
+//        b3FileUtils::findFile("ditto.vtk", relative_path, 1024);
+//        b3FileUtils::findFile("boot.vtk", relative_path, 1024);
+//        btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
+//                                                                  TetraCube::getElements(),
+//                                                                  0,
+//                                                                  TetraCube::getNodes(),
+//                                                                  false, true, true);
+        btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), relative_path);
+
+//        psb->scale(btVector3(30, 30, 30)); // for banana
+        psb->scale(btVector3(.25, .25, .25));
+//        psb->scale(btVector3(.3, .3, .3));  // for tube, torus, boot
+//        psb->scale(btVector3(1, 1, 1));  // for ditto
+//        psb->translate(btVector3(0, 0, 2)); for boot
+        psb->getCollisionShape()->setMargin(0.01);
+        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.collisions = btSoftBody::fCollision::SDF_RD;
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+        psb->getWorldInfo()->m_maxDisplacement = .1f;
+        // nonlinear damping 
+//        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(1,0.04, true));
+//        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
+//        getDeformableDynamicsWorld()->addForce(psb, new btDeformableCorotatedForce(0,6));
+        
+        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.5,0.04, true);
+        getDeformableDynamicsWorld()->addForce(psb, mass_spring);
+        forces.push_back(mass_spring);
+        
+        btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(gravity);
+        getDeformableDynamicsWorld()->addForce(psb, gravity_force);
+        forces.push_back(gravity_force);
+        
+        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(2,10);
+        getDeformableDynamicsWorld()->addForce(psb, neohookean);
+        forces.push_back(neohookean);
+    }
+    
+//    // create a piece of cloth
+//    {
+//        bool onGround = false;
+//        const btScalar s = 4;
+//        btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
+//                                                         btVector3(+s, 0, -s),
+//                                                         btVector3(-s, 0, +s),
+//                                                         btVector3(+s, 0, +s),
+//                                                         20,20,
+//                                                                  0, true);
+//
+//        if (onGround)
+//            psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
+//                                                 btVector3(+s, 0, -s),
+//                                                 btVector3(-s, 0, +s),
+//                                                 btVector3(+s, 0, +s),
+//                                                 //                                                 20,20,
+//                                                 2,2,
+//                                                 0, true);
+//
+//        psb->getCollisionShape()->setMargin(0.1);
+//        psb->generateBendingConstraints(2);
+//        psb->setTotalMass(1);
+//        psb->setSpringStiffness(2);
+//        psb->setDampingCoefficient(0.03);
+//        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.collisions = btSoftBody::fCollision::SDF_RD;
+//        getDeformableDynamicsWorld()->addSoftBody(psb);
+//        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
+//        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
+//    }
+    
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+    
+    {
+        SliderParams slider("Moving velocity", &sGripperVerticalVelocity);
+        slider.m_minVal = -.2;
+        slider.m_maxVal = .2;
+        m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+    
+    {
+        SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
+        slider.m_minVal = -.1;
+        slider.m_maxVal = .1;
+        m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+    
+}
+
+void GraspDeformable::exitPhysics()
+{
+	//cleanup in the reverse order of creation/initialization
+
+	//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 < forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = forces[j];
+        delete force;
+    }
+	//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;
+}
+
+btMultiBody* GraspDeformable::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool floating)
+{
+    //init the base
+    btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
+    float baseMass = 1.f;
+    float linkMass = 1.f;
+    int numLinks = 2;
+    
+    if (baseMass)
+    {
+        btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
+        pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
+        delete pTempBox;
+    }
+    
+    bool canSleep = false;
+    btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
+    
+    btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
+    pMultiBody->setBasePos(basePosition);
+    pMultiBody->setWorldToBaseRot(baseOriQuat);
+    
+    //init the links
+    btVector3 hingeJointAxis(1, 0, 0);
+    
+    btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
+    
+    btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
+    pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
+    delete pTempBox;
+    
+    //y-axis assumed up
+    btAlignedObjectArray<btVector3> parentComToCurrentCom;
+    parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, -baseHalfExtents[2] * 4.f));
+    parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, +baseHalfExtents[2] * 4.f));//par body's COM to cur body's COM offset
+    
+    btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1]*8.f, 0);                         //cur body's COM to cur body's PIV offset
+    
+    btAlignedObjectArray<btVector3> parentComToCurrentPivot;
+    parentComToCurrentPivot.push_back(btVector3(parentComToCurrentCom[0] - currentPivotToCurrentCom));
+    parentComToCurrentPivot.push_back(btVector3(parentComToCurrentCom[1] - currentPivotToCurrentCom));//par body's COM to cur body's PIV offset
+    
+    //////
+    btScalar q0 = 0.f * SIMD_PI / 180.f;
+    btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
+    quat0.normalize();
+    /////
+    
+    for (int i = 0; i < numLinks; ++i)
+    {
+        pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot[i], currentPivotToCurrentCom, true);
+    }
+    pMultiBody->finalizeMultiDof();
+    ///
+    pWorld->addMultiBody(pMultiBody);
+    ///
+    return pMultiBody;
+}
+
+void GraspDeformable::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
+{
+    btAlignedObjectArray<btQuaternion> world_to_local;
+    world_to_local.resize(pMultiBody->getNumLinks() + 1);
+    
+    btAlignedObjectArray<btVector3> local_origin;
+    local_origin.resize(pMultiBody->getNumLinks() + 1);
+    world_to_local[0] = pMultiBody->getWorldToBaseRot();
+    local_origin[0] = pMultiBody->getBasePos();
+    
+    {
+        btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
+        
+        if (1)
+        {
+            btCollisionShape* box = new btBoxShape(baseHalfExtents);
+            btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
+            col->setCollisionShape(box);
+            
+            btTransform tr;
+            tr.setIdentity();
+            tr.setOrigin(local_origin[0]);
+            tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+            col->setWorldTransform(tr);
+            
+            pWorld->addCollisionObject(col, 2, 1 + 2);
+            
+            col->setFriction(friction);
+            pMultiBody->setBaseCollider(col);
+        }
+    }
+    
+    for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
+    {
+        const int parent = pMultiBody->getParent(i);
+        world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
+        local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
+    }
+    
+    for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
+    {
+        btVector3 posr = local_origin[i + 1];
+        
+        btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
+        
+        btCollisionShape* box = new btBoxShape(linkHalfExtents);
+        btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
+        
+        col->setCollisionShape(box);
+        btTransform tr;
+        tr.setIdentity();
+        tr.setOrigin(posr);
+        tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+        col->setWorldTransform(tr);
+        col->setFriction(friction);
+        pWorld->addCollisionObject(col, 2, 1 + 2);
+        
+        pMultiBody->getLink(i).m_collider = col;
+    }
+}
+
+class CommonExampleInterface* GraspDeformableCreateFunc(struct CommonExampleOptions& options)
+{
+	return new GraspDeformable(options.m_guiHelper);
+}
+
+

examples/DeformableDemo/GraspDeformable.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 _GRASP_DEFORMABLE_H
+#define _GRASP_DEFORMABLE_H
+
+class CommonExampleInterface* GraspDeformableCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_GRASP_DEFORMABLE_H

examples/DeformableDemo/Pinch.cpp

@@ -28,7 +28,7 @@
 #include "Pinch.h"
 ///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
 #include "btBulletDynamicsCommon.h"
-#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
 #include "BulletSoftBody/btSoftBody.h"
 #include "BulletSoftBody/btSoftBodyHelpers.h"
 #include "BulletSoftBody/btDeformableBodySolver.h"
@@ -56,6 +56,7 @@ struct TetraBunny
 
 class Pinch : public CommonRigidBodyBase
 {
+    btAlignedObjectArray<btDeformableLagrangianForce*> forces;
 public:
 	Pinch(struct GUIHelperInterface* helper)
 		: CommonRigidBodyBase(helper)
@@ -102,20 +103,20 @@ public:
         }
     }
     
-    virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
+    virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
     {
-        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
     }
     
-    virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld()
+    virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
     {
-        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
     }
     
     virtual void renderScene()
     {
         CommonRigidBodyBase::renderScene();
-        btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
         
         for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
         {
@@ -128,7 +129,7 @@ public:
     }
 };
 
-void dynamics(btScalar time, btDeformableRigidDynamicsWorld* world)
+void dynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
 {
     btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
     if (rbs.size()<2)
@@ -244,12 +245,11 @@ void Pinch::initPhysics()
 	m_broadphase = new btDbvtBroadphase();
     btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
 
-	btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver();
+	btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(deformableBodySolver);
 	m_solver = sol;
 
-	m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
+	m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
-    deformableBodySolver->setWorld(getDeformableDynamicsWorld());
-	//	m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
     btVector3 gravity = btVector3(0, -10, 0);
 	m_dynamicsWorld->setGravity(gravity);
     getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
@@ -331,19 +331,23 @@ void Pinch::initPhysics()
         psb->translate(btVector3(0, 4, 0));
         psb->getCollisionShape()->setMargin(0.1);
         psb->setTotalMass(1);
-//        psb->scale(btVector3(5, 5, 5));
-//        psb->translate(btVector3(-2.5, 4, -2.5));
-//        psb->getCollisionShape()->setMargin(0.1);
-//        psb->setTotalMass(1);
-        psb->setSpringStiffness(2);
-        psb->setDampingCoefficient(0.02);
         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.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addSoftBody(psb);
-        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
+        
-        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
+        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(1,0.05);
+        getDeformableDynamicsWorld()->addForce(psb, mass_spring);
+        forces.push_back(mass_spring);
+        
+        btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(gravity);
+        getDeformableDynamicsWorld()->addForce(psb, gravity_force);
+        forces.push_back(gravity_force);
+        
+        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.2,1);
+        getDeformableDynamicsWorld()->addForce(psb, neohookean);
+        forces.push_back(neohookean);
         // add a grippers
         createGrip();
     }
@@ -367,7 +371,12 @@ void Pinch::exitPhysics()
 		m_dynamicsWorld->removeCollisionObject(obj);
 		delete obj;
 	}
-
+    // delete forces
+    for (int j = 0; j < forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = forces[j];
+        delete force;
+    }
 	//delete collision shapes
 	for (int j = 0; j < m_collisionShapes.size(); j++)
 	{

examples/DeformableDemo/VolumetricDeformable.cpp

@@ -28,7 +28,7 @@
 #include "VolumetricDeformable.h"
 ///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
 #include "btBulletDynamicsCommon.h"
-#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
 #include "BulletSoftBody/btSoftBody.h"
 #include "BulletSoftBody/btSoftBodyHelpers.h"
 #include "BulletSoftBody/btDeformableBodySolver.h"
@@ -51,6 +51,7 @@ struct TetraCube
 
 class VolumetricDeformable : public CommonRigidBodyBase
 {
+    btAlignedObjectArray<btDeformableLagrangianForce*> forces;
 public:
 	VolumetricDeformable(struct GUIHelperInterface* helper)
 		: CommonRigidBodyBase(helper)
@@ -133,24 +134,24 @@ public:
         }
     }
     
-    virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
+    virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
     {
         ///just make it a btSoftRigidDynamicsWorld please
         ///or we will add type checking
-        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
     }
     
-    virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld()
+    virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
     {
         ///just make it a btSoftRigidDynamicsWorld please
         ///or we will add type checking
-        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
     }
     
     virtual void renderScene()
     {
         CommonRigidBodyBase::renderScene();
-        btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
         
         for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
         {
@@ -177,13 +178,11 @@ void VolumetricDeformable::initPhysics()
 	m_broadphase = new btDbvtBroadphase();
     btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
 
-	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
+	btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
-	btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver();
+    sol->setDeformableSolver(deformableBodySolver);
 	m_solver = sol;
 
-	m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
+	m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
-    deformableBodySolver->setWorld(getDeformableDynamicsWorld());
-	//	m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
     btVector3 gravity = btVector3(0, -10, 0);
 	m_dynamicsWorld->setGravity(gravity);
     getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
@@ -230,18 +229,25 @@ void VolumetricDeformable::initPhysics()
         getDeformableDynamicsWorld()->addSoftBody(psb);
         psb->scale(btVector3(2, 2, 2));
         psb->translate(btVector3(0, 5, 0));
-//        psb->setVolumeMass(10);
         psb->getCollisionShape()->setMargin(0.25);
-//        psb->generateBendingConstraints(2);
         psb->setTotalMass(1);
-        psb->setSpringStiffness(1);
-        psb->setDampingCoefficient(0.01);
         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;
-        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
+        
-        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
+        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(0,0.03);
+        getDeformableDynamicsWorld()->addForce(psb, mass_spring);
+        forces.push_back(mass_spring);
+        
+        btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(gravity);
+        getDeformableDynamicsWorld()->addForce(psb, gravity_force);
+        forces.push_back(gravity_force);
+        
+        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.5,2.5);
+        getDeformableDynamicsWorld()->addForce(psb, neohookean);
+        forces.push_back(neohookean);
+        
     }
     // add a few rigid bodies
     Ctor_RbUpStack(4); 
@@ -266,6 +272,12 @@ void VolumetricDeformable::exitPhysics()
 		m_dynamicsWorld->removeCollisionObject(obj);
 		delete obj;
 	}
+    // delete forces
+    for (int j = 0; j < forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = forces[j];
+        delete force;
+    }
 
 	//delete collision shapes
 	for (int j = 0; j < m_collisionShapes.size(); j++)

examples/ExampleBrowser/CMakeLists.txt

@@ -359,6 +359,8 @@ SET(BulletExampleBrowser_SRCS
  ../MultiBody/MultiBodyConstraintFeedback.cpp
   ../SoftDemo/SoftDemo.cpp
   ../SoftDemo/SoftDemo.h
+  ../DeformableDemo/GraspDeformable.cpp
+  ../DeformableDemo/GraspDeformable.h
   ../DeformableDemo/Pinch.cpp
   ../DeformableDemo/Pinch.h
   ../DeformableDemo/DeformableMultibody.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -51,6 +51,7 @@
 #include "../DeformableDemo/Pinch.h"
 #include "../DeformableDemo/DeformableMultibody.h"
 #include "../DeformableDemo/VolumetricDeformable.h"
+#include "../DeformableDemo/GraspDeformable.h"
 #include "../SharedMemory/PhysicsServerExampleBullet2.h"
 #include "../SharedMemory/PhysicsServerExample.h"
 #include "../SharedMemory/PhysicsClientExample.h"
@@ -197,6 +198,7 @@ static ExampleEntry gDefaultExamples[] =
         ExampleEntry(0, "Deformabe Body"),
         ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableRigidCreateFunc),
         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, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc),
         // ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc),

src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h

@@ -27,14 +27,20 @@ public:
 	const btCollisionShape* m_shape;
 	const btCollisionObject* m_collisionObject;
 	const btTransform& m_worldTransform;
+    const btTransform* m_preTransform;
 	int m_partId;
 	int m_index;
 
 	btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, int partId, int index)
-		: m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_partId(partId), m_index(index)
+		: m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_preTransform(NULL), m_partId(partId), m_index(index)
 	{
 	}
     
+    btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, const btTransform& preTransform, int partId, int index)
+    : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_preTransform(&preTransform), m_partId(partId), m_index(index)
+    {
+    }
+
 	SIMD_FORCE_INLINE const btTransform& getWorldTransform() const { return m_worldTransform; }
 	SIMD_FORCE_INLINE const btCollisionObject* getCollisionObject() const { return m_collisionObject; }
 	SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_shape; }

src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

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

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -338,13 +338,17 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
 	}
 }
 
+
 ///@todo: this is random access, it can be walked 'cache friendly'!
 void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback)
 {
-	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
-
 	buildIslands(dispatcher, collisionWorld);
+    processIslands(dispatcher, collisionWorld, callback);
+}
 
+void btSimulationIslandManager::processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback)
+{
+    btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
 	int endIslandIndex = 1;
 	int startIslandIndex;
 	int numElem = getUnionFind().getNumElements();

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h

@@ -60,6 +60,8 @@ public:
     
 	void buildIslands(btDispatcher* dispatcher, btCollisionWorld* colWorld);
 
+    void processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback);
+    
 	bool getSplitIslands()
 	{
 		return m_splitIslands;

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -1707,9 +1707,9 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
             {
                 //reset to current pos
 
-                for (int i = 0; i < 4; ++i)
+                for (int j = 0; j < 4; ++j)
                 {
-                    pJointPos[i] = m_links[i].m_jointPos[i];
+                    pJointPos[j] = m_links[i].m_jointPos[j];
                 }
                 
                 btVector3 jointVel;
@@ -1725,9 +1725,9 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
             }
             case btMultibodyLink::ePlanar:
             {
-                for (int i = 0; i < 3; ++i)
+                for (int j = 0; j < 3; ++j)
                 {
-                    pJointPos[i] = m_links[i].m_jointPos[i];
+                    pJointPos[j] = m_links[i].m_jointPos[j];
                 }
                 pJointPos[0] += dt * getJointVelMultiDof(i)[0];
                 
@@ -2142,6 +2142,7 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQu
 		tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
 
 		getBaseCollider()->setWorldTransform(tr);
+        getBaseCollider()->setInterpolationWorldTransform(tr);
 	}
 
 	for (int k = 0; k < getNumLinks(); k++)
@@ -2170,6 +2171,7 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQu
 			tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
 
 			col->setWorldTransform(tr);
+            col->setInterpolationWorldTransform(tr);
 		}
 	}
 }

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -273,6 +273,11 @@ public:
 	{
 		return &m_realBuf[0];
 	}
+    
+    const btScalar *getDeltaVelocityVector() const
+    {
+        return &m_deltaV[0];
+    }
 	/*    btScalar * getVelocityVector() 
 	{ 
 		return &real_buf[0]; 

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -169,218 +169,6 @@ void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
 	btDiscreteDynamicsWorld::updateActivationState(timeStep);
 }
 
-SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs)
-{
-	int islandId;
-
-	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
-	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
-	islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
-	return islandId;
-}
-
-class btSortConstraintOnIslandPredicate2
-{
-public:
-	bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const
-	{
-		int rIslandId0, lIslandId0;
-		rIslandId0 = btGetConstraintIslandId2(rhs);
-		lIslandId0 = btGetConstraintIslandId2(lhs);
-		return lIslandId0 < rIslandId0;
-	}
-};
-
-SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
-{
-	int islandId;
-
-	int islandTagA = lhs->getIslandIdA();
-	int islandTagB = lhs->getIslandIdB();
-	islandId = islandTagA >= 0 ? islandTagA : islandTagB;
-	return islandId;
-}
-
-class btSortMultiBodyConstraintOnIslandPredicate
-{
-public:
-	bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const
-	{
-		int rIslandId0, lIslandId0;
-		rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
-		lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
-		return lIslandId0 < rIslandId0;
-	}
-};
-
-
-struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
-{
-	btContactSolverInfo* m_solverInfo;
-	btMultiBodyConstraintSolver* m_solver;
-	btMultiBodyConstraint** m_multiBodySortedConstraints;
-	int m_numMultiBodyConstraints;
-
-	btTypedConstraint** m_sortedConstraints;
-	int m_numConstraints;
-	btIDebugDraw* m_debugDrawer;
-	btDispatcher* m_dispatcher;
-
-	btAlignedObjectArray<btCollisionObject*> m_bodies;
-	btAlignedObjectArray<btPersistentManifold*> m_manifolds;
-	btAlignedObjectArray<btTypedConstraint*> m_constraints;
-	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
-
-	btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData;
-
-	MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver,
-										 btDispatcher* dispatcher)
-		: m_solverInfo(NULL),
-		  m_solver(solver),
-		  m_multiBodySortedConstraints(NULL),
-		  m_numConstraints(0),
-		  m_debugDrawer(NULL),
-		  m_dispatcher(dispatcher)
-	{
-	}
-
-	MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other)
-	{
-		btAssert(0);
-		(void)other;
-		return *this;
-	}
-
-	SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
-	{
-		m_islandAnalyticsData.clear();
-		btAssert(solverInfo);
-		m_solverInfo = solverInfo;
-
-		m_multiBodySortedConstraints = sortedMultiBodyConstraints;
-		m_numMultiBodyConstraints = numMultiBodyConstraints;
-		m_sortedConstraints = sortedConstraints;
-		m_numConstraints = numConstraints;
-
-		m_debugDrawer = debugDrawer;
-		m_bodies.resize(0);
-		m_manifolds.resize(0);
-		m_constraints.resize(0);
-		m_multiBodyConstraints.resize(0);
-	}
-
-	void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
-	{
-		m_solver = solver;
-	}
-
-	virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId)
-	{
-		if (islandId < 0)
-		{
-			///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
-			m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
-			if (m_solverInfo->m_reportSolverAnalytics&1)
-			{
-				m_solver->m_analyticsData.m_islandId = islandId;
-				m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
-			}
-		}
-		else
-		{
-			//also add all non-contact constraints/joints for this island
-			btTypedConstraint** startConstraint = 0;
-			btMultiBodyConstraint** startMultiBodyConstraint = 0;
-
-			int numCurConstraints = 0;
-			int numCurMultiBodyConstraints = 0;
-
-			int i;
-
-			//find the first constraint for this island
-
-			for (i = 0; i < m_numConstraints; i++)
-			{
-				if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
-				{
-					startConstraint = &m_sortedConstraints[i];
-					break;
-				}
-			}
-			//count the number of constraints in this island
-			for (; i < m_numConstraints; i++)
-			{
-				if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
-				{
-					numCurConstraints++;
-				}
-			}
-
-			for (i = 0; i < m_numMultiBodyConstraints; i++)
-			{
-				if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
-				{
-					startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
-					break;
-				}
-			}
-			//count the number of multi body constraints in this island
-			for (; i < m_numMultiBodyConstraints; i++)
-			{
-				if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
-				{
-					numCurMultiBodyConstraints++;
-				}
-			}
-
-			//if (m_solverInfo->m_minimumSolverBatchSize<=1)
-			//{
-			//	m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
-			//} else
-			{
-				for (i = 0; i < numBodies; i++)
-					m_bodies.push_back(bodies[i]);
-				for (i = 0; i < numManifolds; i++)
-					m_manifolds.push_back(manifolds[i]);
-				for (i = 0; i < numCurConstraints; i++)
-					m_constraints.push_back(startConstraint[i]);
-
-				for (i = 0; i < numCurMultiBodyConstraints; i++)
-					m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
-
-				if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
-				{
-					processConstraints(islandId);
-				}
-				else
-				{
-					//printf("deferred\n");
-				}
-			}
-		}
-	}
-	void processConstraints(int islandId=-1)
-	{
-		btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
-		btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
-		btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
-		btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
-
-		//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
-
-		m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
-		if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1))
-		{
-			m_solver->m_analyticsData.m_islandId = islandId;
-			m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
-		}
-		m_bodies.resize(0);
-		m_manifolds.resize(0);
-		m_constraints.resize(0);
-		m_multiBodyConstraints.resize(0);
-	}
-};
-
 void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const
 {
 	islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData;
@@ -441,56 +229,53 @@ void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& sol
 {
 	/// solve all the constraints for this island
 	m_solverMultiBodyIslandCallback->processConstraints();
-
 	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
+    {
+        BT_PROFILE("btMultiBody stepVelocities");
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
             
-	{
+            bool isSleeping = false;
-		BT_PROFILE("btMultiBody stepVelocities");
-		for (int i = 0; i < this->m_multiBodies.size(); i++)
-		{
-			btMultiBody* bod = m_multiBodies[i];
             
-			bool isSleeping = false;
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
             
-			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            if (!isSleeping)
-			{
+            {
-				isSleeping = true;
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
-			}
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
-			for (int b = 0; b < bod->getNumLinks(); b++)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
-			{
+                m_scratch_m.resize(bod->getNumLinks() + 1);
-				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
-					isSleeping = true;
-			}
                 
-			if (!isSleeping)
+                if (bod->internalNeedsJointFeedback())
-			{
+                {
-				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                    if (!bod->isUsingRK4Integration())
-				m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                    {
-				m_scratch_v.resize(bod->getNumLinks() + 1);
+                        if (bod->internalNeedsJointFeedback())
-				m_scratch_m.resize(bod->getNumLinks() + 1);
+                        {
-
+                            bool isConstraintPass = true;
-				if (bod->internalNeedsJointFeedback())
+                            bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
-				{
+                                                                                      getSolverInfo().m_jointFeedbackInWorldSpace,
-					if (!bod->isUsingRK4Integration())
+                                                                                      getSolverInfo().m_jointFeedbackInJointFrame);
-					{
+                        }
-						if (bod->internalNeedsJointFeedback())
+                    }
-						{
+                }
-							bool isConstraintPass = true;
+            }
-							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+        }
-								getSolverInfo().m_jointFeedbackInWorldSpace,
+    }
-								getSolverInfo().m_jointFeedbackInJointFrame);
+    for (int i = 0; i < this->m_multiBodies.size(); i++)
-						}
+    {
-					}
+        btMultiBody* bod = m_multiBodies[i];
-				}
+        bod->processDeltaVeeMultiDof2();
-			}
+    }
-		}
-	}
-
-	for (int i = 0; i < this->m_multiBodies.size(); i++)
-	{
-		btMultiBody* bod = m_multiBodies[i];
-		bod->processDeltaVeeMultiDof2();
-	}
 }
 
 void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo)
@@ -588,15 +373,10 @@ void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverIn
                 {
                     if (!bod->isUsingRK4Integration())
                     {
-                        const btScalar linearDamp = bod->getLinearDamping();
-//                        const btScalar angularDamp = bod->getAngularDamping();
-                        bod->setLinearDamping(0);
                         bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
                                                                                   m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
                                                                                   getSolverInfo().m_jointFeedbackInWorldSpace,
                                                                                   getSolverInfo().m_jointFeedbackInJointFrame);
-                        bod->setLinearDamping(linearDamp);
-//                        bod->setAngularDamping(angularDamp);
                     }
                     else
                     {
@@ -1087,3 +867,8 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
 		}
 	}
 }
+//
+//void btMultiBodyDynamicsWorld::setSplitIslands(bool split)
+//{
+//    m_islandManager->setSplitIslands(split);
+//}

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -17,6 +17,7 @@ subject to the following restrictions:
 #define BT_MULTIBODY_DYNAMICS_WORLD_H
 
 #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h"
 
 #define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 
@@ -57,6 +58,7 @@ public:
 	virtual ~btMultiBodyDynamicsWorld();
     
     virtual void solveConstraints(btContactSolverInfo& solverInfo);
+    
 	virtual void addMultiBody(btMultiBody* body, int group = btBroadphaseProxy::DefaultFilter, int mask = btBroadphaseProxy::AllFilter);
 
 	virtual void removeMultiBody(btMultiBody* body);
@@ -118,6 +120,5 @@ public:
     virtual void solveExternalForces(btContactSolverInfo& solverInfo);
     virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
     void buildIslands();
-
 };
 #endif  //BT_MULTIBODY_DYNAMICS_WORLD_H

src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h

@@ -0,0 +1,235 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H
+#define BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H
+
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "btMultiBodyConstraintSolver.h"
+
+SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs)
+{
+    int islandId;
+    
+    const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
+    const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
+    islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
+    return islandId;
+}
+class btSortConstraintOnIslandPredicate2
+{
+public:
+    bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const
+    {
+        int rIslandId0, lIslandId0;
+        rIslandId0 = btGetConstraintIslandId2(rhs);
+        lIslandId0 = btGetConstraintIslandId2(lhs);
+        return lIslandId0 < rIslandId0;
+    }
+};
+
+SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
+{
+    int islandId;
+    
+    int islandTagA = lhs->getIslandIdA();
+    int islandTagB = lhs->getIslandIdB();
+    islandId = islandTagA >= 0 ? islandTagA : islandTagB;
+    return islandId;
+}
+
+class btSortMultiBodyConstraintOnIslandPredicate
+{
+public:
+    bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const
+    {
+        int rIslandId0, lIslandId0;
+        rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
+        lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
+        return lIslandId0 < rIslandId0;
+    }
+};
+
+struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
+{
+
+    btContactSolverInfo* m_solverInfo;
+    btMultiBodyConstraintSolver* m_solver;
+    btMultiBodyConstraint** m_multiBodySortedConstraints;
+    int m_numMultiBodyConstraints;
+    
+    btTypedConstraint** m_sortedConstraints;
+    int m_numConstraints;
+    btIDebugDraw* m_debugDrawer;
+    btDispatcher* m_dispatcher;
+    
+    btAlignedObjectArray<btCollisionObject*> m_bodies;
+    btAlignedObjectArray<btPersistentManifold*> m_manifolds;
+    btAlignedObjectArray<btTypedConstraint*> m_constraints;
+    btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
+    
+    btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData;
+    
+    MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver,
+                                         btDispatcher* dispatcher)
+    : m_solverInfo(NULL),
+    m_solver(solver),
+    m_multiBodySortedConstraints(NULL),
+    m_numConstraints(0),
+    m_debugDrawer(NULL),
+    m_dispatcher(dispatcher)
+    {
+    }
+    
+    MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other)
+    {
+        btAssert(0);
+        (void)other;
+        return *this;
+    }
+    
+    SIMD_FORCE_INLINE virtual void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
+    {
+        m_islandAnalyticsData.clear();
+        btAssert(solverInfo);
+        m_solverInfo = solverInfo;
+        
+        m_multiBodySortedConstraints = sortedMultiBodyConstraints;
+        m_numMultiBodyConstraints = numMultiBodyConstraints;
+        m_sortedConstraints = sortedConstraints;
+        m_numConstraints = numConstraints;
+        
+        m_debugDrawer = debugDrawer;
+        m_bodies.resize(0);
+        m_manifolds.resize(0);
+        m_constraints.resize(0);
+        m_multiBodyConstraints.resize(0);
+    }
+    
+    void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
+    {
+        m_solver = solver;
+    }
+    
+    virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId)
+    {
+        if (islandId < 0)
+        {
+            ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
+            m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
+            if (m_solverInfo->m_reportSolverAnalytics&1)
+            {
+                m_solver->m_analyticsData.m_islandId = islandId;
+                m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+            }
+        }
+        else
+        {
+            //also add all non-contact constraints/joints for this island
+            btTypedConstraint** startConstraint = 0;
+            btMultiBodyConstraint** startMultiBodyConstraint = 0;
+            
+            int numCurConstraints = 0;
+            int numCurMultiBodyConstraints = 0;
+            
+            int i;
+            
+            //find the first constraint for this island
+            
+            for (i = 0; i < m_numConstraints; i++)
+            {
+                if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
+                {
+                    startConstraint = &m_sortedConstraints[i];
+                    break;
+                }
+            }
+            //count the number of constraints in this island
+            for (; i < m_numConstraints; i++)
+            {
+                if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
+                {
+                    numCurConstraints++;
+                }
+            }
+            
+            for (i = 0; i < m_numMultiBodyConstraints; i++)
+            {
+                if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
+                {
+                    startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
+                    break;
+                }
+            }
+            //count the number of multi body constraints in this island
+            for (; i < m_numMultiBodyConstraints; i++)
+            {
+                if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
+                {
+                    numCurMultiBodyConstraints++;
+                }
+            }
+            
+            //if (m_solverInfo->m_minimumSolverBatchSize<=1)
+            //{
+            //    m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
+            //} else
+            {
+                for (i = 0; i < numBodies; i++)
+                    m_bodies.push_back(bodies[i]);
+                for (i = 0; i < numManifolds; i++)
+                    m_manifolds.push_back(manifolds[i]);
+                for (i = 0; i < numCurConstraints; i++)
+                    m_constraints.push_back(startConstraint[i]);
+                
+                for (i = 0; i < numCurMultiBodyConstraints; i++)
+                    m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
+                
+                if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
+                {
+                    processConstraints(islandId);
+                }
+                else
+                {
+                    //printf("deferred\n");
+                }
+            }
+        }
+    }
+    
+    virtual void processConstraints(int islandId=-1)
+    {
+        btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
+        btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
+        btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
+        btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
+        
+        //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
+        
+        m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
+        if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1))
+        {
+            m_solver->m_analyticsData.m_islandId = islandId;
+            m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+        }
+        m_bodies.resize(0);
+        m_manifolds.resize(0);
+        m_constraints.resize(0);
+        m_multiBodyConstraints.resize(0);
+    }
+};
+
+
+#endif /*BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H */

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -157,6 +157,7 @@ struct btMultibodyLink
 		  m_parent(-1),
 		  m_zeroRotParentToThis(0, 0, 0, 1),
 		  m_cachedRotParentToThis(0, 0, 0, 1),
+          m_cachedRotParentToThis_interpolate(0, 0, 0, 1),
 		  m_collider(0),
 		  m_flags(0),
 		  m_dofCount(0),
@@ -179,6 +180,7 @@ struct btMultibodyLink
 		m_dVector.setValue(0, 0, 0);
 		m_eVector.setValue(0, 0, 0);
 		m_cachedRVector.setValue(0, 0, 0);
+        m_cachedRVector_interpolate.setValue(0, 0, 0);
 		m_appliedForce.setValue(0, 0, 0);
 		m_appliedTorque.setValue(0, 0, 0);
 		m_appliedConstraintForce.setValue(0, 0, 0);
@@ -195,7 +197,7 @@ struct btMultibodyLink
 	{
         btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
         btQuaternion& cachedRot = m_cachedRotParentToThis;
-        btVector3& cachedVector =m_cachedRVector;
+        btVector3& cachedVector = m_cachedRVector;
 		switch (m_jointType)
 		{
 			case eRevolute:
@@ -239,6 +241,8 @@ struct btMultibodyLink
 				btAssert(0);
 			}
 		}
+        m_cachedRotParentToThis_interpolate = m_cachedRotParentToThis;
+        m_cachedRVector_interpolate = m_cachedRVector;
 	}
     
     void updateInterpolationCacheMultiDof()

src/BulletSoftBody/CMakeLists.txt

@@ -16,11 +16,12 @@ SET(BulletSoftBody_SRCS
 	btSoftMultiBodyDynamicsWorld.cpp
 	btSoftSoftCollisionAlgorithm.cpp
 	btDefaultSoftBodySolver.cpp
+	btDeformableMultiBodyConstraintSolver.cpp
 
 	btDeformableBackwardEulerObjective.cpp
 	btDeformableBodySolver.cpp
 	btDeformableContactProjection.cpp
-	btDeformableRigidDynamicsWorld.cpp
+	btDeformableMultiBodyDynamicsWorld.cpp
 
 )
 
@@ -43,13 +44,16 @@ SET(BulletSoftBody_HDRS
 	btConjugateGradient.h
 	btDeformableGravityForce.h
 	btDeformableMassSpringForce.h
+	btDeformableCorotatedForce.h
+	btDeformableNeoHookeanForce.h
 	btDeformableLagrangianForce.h
 	btPreconditioner.h
 
 	btDeformableBackwardEulerObjective.h
 	btDeformableBodySolver.h
+	btDeformableMultiBodyConstraintSolver.h
 	btDeformableContactProjection.h
-	btDeformableRigidDynamicsWorld.h
+	btDeformableMultiBodyDynamicsWorld.h
 
 	btSoftBodySolverVertexBuffer.h
 )

src/BulletSoftBody/btCGProjection.h

@@ -1,4 +1,6 @@
 /*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
  Bullet Continuous Collision Detection and Physics Library
  Copyright (c) 2019 Google Inc. http://bulletphysics.org
  This software is provided 'as-is', without any express or implied warranty.
@@ -18,43 +20,34 @@
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 
-class btDeformableRigidDynamicsWorld;
+//class btDeformableMultiBodyDynamicsWorld;
 
 struct DeformableContactConstraint
 {
+    const btSoftBody::Node* m_node;
     btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
-    btAlignedObjectArray<btVector3> m_direction;
+    btAlignedObjectArray<btVector3> m_total_normal_dv;
-    btAlignedObjectArray<btScalar> m_value;
+    btAlignedObjectArray<btVector3> m_total_tangent_dv;
-    // the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve
+    btAlignedObjectArray<bool> m_static;
-    btAlignedObjectArray<btScalar> m_accumulated_normal_impulse;
+    btAlignedObjectArray<bool> m_can_be_dynamic;
     
-    DeformableContactConstraint(const btSoftBody::RContact& rcontact)
+    DeformableContactConstraint(const btSoftBody::RContact& rcontact): m_node(rcontact.m_node)
     {
         append(rcontact);
     }
     
-    DeformableContactConstraint(const btVector3& dir)
+    DeformableContactConstraint(): m_node(NULL)
     {
         m_contact.push_back(NULL);
-        m_direction.push_back(dir);
-        m_value.push_back(0);
-        m_accumulated_normal_impulse.push_back(0);
-    }
-    
-    DeformableContactConstraint()
-    {
-        m_contact.push_back(NULL);
-        m_direction.push_back(btVector3(0,0,0));
-        m_value.push_back(0);
-        m_accumulated_normal_impulse.push_back(0);
     }
     
     void append(const btSoftBody::RContact& rcontact)
     {
         m_contact.push_back(&rcontact);
-        m_direction.push_back(rcontact.m_cti.m_normal);
+        m_total_normal_dv.push_back(btVector3(0,0,0));
-        m_value.push_back(0);
+        m_total_tangent_dv.push_back(btVector3(0,0,0));
-        m_accumulated_normal_impulse.push_back(0);
+        m_static.push_back(false);
+        m_can_be_dynamic.push_back(true);
     }
     
     ~DeformableContactConstraint()
@@ -62,49 +55,6 @@ struct DeformableContactConstraint
     }
 };
 
-struct DeformableFrictionConstraint
-{
-    btAlignedObjectArray<bool> m_static; // whether the friction is static
-    btAlignedObjectArray<btScalar> m_impulse; // the impulse magnitude the node feels
-    btAlignedObjectArray<btScalar> m_dv;      // the dv magnitude of the node
-    btAlignedObjectArray<btVector3> m_direction; // the direction of the friction for the node
-    
-    
-    btAlignedObjectArray<bool> m_static_prev;
-    btAlignedObjectArray<btScalar> m_impulse_prev;
-    btAlignedObjectArray<btScalar> m_dv_prev;
-    btAlignedObjectArray<btVector3> m_direction_prev;
-    
-    btAlignedObjectArray<bool> m_released; // whether the contact is released
-
-    
-    // the total impulse the node applied to the rb in the tangential direction in the cg solve
-    btAlignedObjectArray<btVector3> m_accumulated_tangent_impulse;
-    
-    DeformableFrictionConstraint()
-    {
-        append();
-    }
-    
-    void append()
-    {
-        m_static.push_back(false);
-        m_static_prev.push_back(false);
-        
-        m_direction_prev.push_back(btVector3(0,0,0));
-        m_direction.push_back(btVector3(0,0,0));
-        
-        m_impulse.push_back(0);
-        m_impulse_prev.push_back(0);
-        
-        m_dv.push_back(0);
-        m_dv_prev.push_back(0);
-        
-        m_accumulated_tangent_impulse.push_back(btVector3(0,0,0));
-        m_released.push_back(false);
-    }
-};
-
 class btCGProjection
 {
 public:
@@ -112,8 +62,6 @@ public:
     typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
     typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
     btAlignedObjectArray<btSoftBody *>& m_softBodies;
-    btDeformableRigidDynamicsWorld* m_world;
-//    const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
     const btScalar& m_dt;
     
     btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
@@ -132,16 +80,11 @@ public:
     virtual void setConstraints() = 0;
     
     // update the constraints
-    virtual void update() = 0;
+    virtual btScalar update() = 0;
     
     virtual void reinitialize(bool nodeUpdated)
     {
     }
-    
-    virtual void setWorld(btDeformableRigidDynamicsWorld* world)
-    {
-        m_world = world;
-    }
 };
 
 

src/BulletSoftBody/btConjugateGradient.h

@@ -1,4 +1,6 @@
 /*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
  Bullet Continuous Collision Detection and Physics Library
  Copyright (c) 2019 Google Inc. http://bulletphysics.org
  This software is provided 'as-is', without any express or implied warranty.
@@ -33,63 +35,57 @@ public:
     virtual ~btConjugateGradient(){}
     
     // return the number of iterations taken
-    int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance)
+    int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance, bool verbose = false)
     {
         BT_PROFILE("CGSolve");
         btAssert(x.size() == b.size());
         reinitialize(b);
-        
         // r = b - A * x --with assigned dof zeroed out
         A.multiply(x, temp);
         r = sub(b, temp);
         A.project(r);
-        A.enforceConstraint(x);
-        
-        btScalar r_norm = std::sqrt(squaredNorm(r));
-        if (r_norm < tolerance) {
-            std::cout << "Iteration = 0" << std::endl;
-            std::cout << "Two norm of the residual = " << r_norm << std::endl;
-            return 0;
-        }
-        
         // z = M^(-1) * r
         A.precondition(r, z);
-        p = z;
+        A.project(z);
-        // temp = A*p
+        btScalar r_dot_z = dot(z,r);
-        A.multiply(p, temp);
+        if (r_dot_z < tolerance) {
-        
+            if (verbose)
-        btScalar r_dot_z = dot(z,r), r_dot_z_new;
+            {
-        // alpha = r^T * z / (p^T * A * p)
+                std::cout << "Iteration = 0" << std::endl;
-        btScalar alpha = r_dot_z / dot(p, temp), beta;
+                std::cout << "Two norm of the residual = " << r_dot_z << std::endl;
-        
-        for (int k = 1; k < max_iterations; k++) {
-            //  x += alpha * p;
-            //  r -= alpha * temp;
-            multAndAddTo(alpha, p, x);
-            multAndAddTo(-alpha, temp, r);
-            // zero out the dofs of r
-            A.project(r);
-            A.enforceConstraint(x);
-            r_norm = std::sqrt(squaredNorm(r));
-            
-            if (r_norm < tolerance) {
-                std::cout << "ConjugateGradient iterations " << k << std::endl;
-                return k;
             }
-            
+            return 0;
+        }
+        p = z;
+        btScalar r_dot_z_new = r_dot_z;
+        for (int k = 1; k < max_iterations; k++) {
+            // temp = A*p
+            A.multiply(p, temp);
+            A.project(temp);
+            // alpha = r^T * z / (p^T * A * p)
+            btScalar alpha = r_dot_z_new / dot(p, temp);
+            //  x += alpha * p;
+            multAndAddTo(alpha, p, x);
+            //  r -= alpha * temp;
+            multAndAddTo(-alpha, temp, r);
             // z = M^(-1) * r
             A.precondition(r, z);
-            r_dot_z_new = dot(r,z);
-            beta = r_dot_z_new/ r_dot_z;
             r_dot_z = r_dot_z_new;
-            // p = z + beta * p;
+            r_dot_z_new = dot(r,z);
+            if (r_dot_z_new < tolerance) {
+                if (verbose)
+                {
+                    std::cout << "ConjugateGradient iterations " << k << std::endl;
+                }
+                return k;
+            }
+            btScalar beta = r_dot_z_new/ r_dot_z;
             p = multAndAdd(beta, p, z);
-            // temp = A * p;
-            A.multiply(p, temp);
-            // alpha = r^T * z / (p^T * A * p)
-            alpha = r_dot_z / dot(p, temp);
         }
-        std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
+        if (verbose)
+        {
+            std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
+        }
         return max_iterations;
     }
     

src/BulletSoftBody/btDefaultSoftBodySolver.cpp

@@ -60,7 +60,7 @@ bool btDefaultSoftBodySolver::checkInitialized()
 	return true;
 }
 
-void btDefaultSoftBodySolver::solveConstraints(float solverdt)
+void btDefaultSoftBodySolver::solveConstraints(btScalar solverdt)
 {
 	// Solve constraints for non-solver softbodies
 	for (int i = 0; i < m_softBodySet.size(); ++i)
@@ -132,7 +132,7 @@ void btDefaultSoftBodySolver::processCollision(btSoftBody *softBody, const btCol
 	softBody->defaultCollisionHandler(collisionObjectWrap);
 }  // btDefaultSoftBodySolver::processCollision
 
-void btDefaultSoftBodySolver::predictMotion(float timeStep)
+void btDefaultSoftBodySolver::predictMotion(btScalar timeStep)
 {
 	for (int i = 0; i < m_softBodySet.size(); ++i)
 	{

src/BulletSoftBody/btDefaultSoftBodySolver.h

@@ -46,9 +46,9 @@ public:
 
 	virtual void copyBackToSoftBodies(bool bMove = true);
 
-	virtual void solveConstraints(float solverdt);
+	virtual void solveConstraints(btScalar solverdt);
 
-	virtual void predictMotion(float solverdt);
+	virtual void predictMotion(btScalar solverdt);
 
 	virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer);
 

src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp

@@ -1,4 +1,6 @@
 /*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
  Bullet Continuous Collision Detection and Physics Library
  Copyright (c) 2019 Google Inc. http://bulletphysics.org
  This software is provided 'as-is', without any express or implied warranty.
@@ -22,10 +24,18 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned
     m_preconditioner = new DefaultPreconditioner();
 }
 
+btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective()
+{
+    delete m_preconditioner;
+}
+
 void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt)
 {
     BT_PROFILE("reinitialize");
-    setDt(dt);
+    if (dt > 0)
+    {
+        setDt(dt);
+    }
     if(nodeUpdated)
     {
         updateId();
@@ -117,6 +127,11 @@ btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual
 
 void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
 {
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        m_softBodies[i]->updateDeformation();
+    }
+    
     for (int i = 0; i < m_lf.size(); ++i)
     {
         m_lf[i]->addScaledExplicitForce(m_dt, force);
@@ -141,7 +156,10 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack
 //set constraints as projections
 void btDeformableBackwardEulerObjective::setConstraints()
 {
-    // build islands for multibody solve
-    m_world->btMultiBodyDynamicsWorld::buildIslands();
     projection.setConstraints();
 }
+
+void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r)
+{
+     projection.applyDynamicFriction(r);
+}

src/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -1,4 +1,6 @@
 /*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
  Bullet Continuous Collision Detection and Physics Library
  Copyright (c) 2019 Google Inc. http://bulletphysics.org
  This software is provided 'as-is', without any express or implied warranty.
@@ -17,18 +19,18 @@
 #include "btDeformableLagrangianForce.h"
 #include "btDeformableMassSpringForce.h"
 #include "btDeformableGravityForce.h"
+#include "btDeformableCorotatedForce.h"
+#include "btDeformableNeoHookeanForce.h"
 #include "btDeformableContactProjection.h"
 #include "btPreconditioner.h"
-#include "btDeformableRigidDynamicsWorld.h"
+#include "btDeformableMultiBodyDynamicsWorld.h"
 #include "LinearMath/btQuickprof.h"
 
-class btDeformableRigidDynamicsWorld;
 class btDeformableBackwardEulerObjective
 {
 public:
     typedef btAlignedObjectArray<btVector3> TVStack;
     btScalar m_dt;
-    btDeformableRigidDynamicsWorld* m_world;
     btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
     btAlignedObjectArray<btSoftBody *>& m_softBodies;
     Preconditioner* m_preconditioner;
@@ -37,7 +39,7 @@ public:
     btAlignedObjectArray<btSoftBody::Node* > m_nodes;
     btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
     
-    virtual ~btDeformableBackwardEulerObjective() {}
+    virtual ~btDeformableBackwardEulerObjective();
     
     void initialize(){}
     
@@ -72,9 +74,10 @@ public:
     {
         BT_PROFILE("enforceConstraint");
         projection.enforceConstraint(x);
-        updateVelocity(x);
     }
     
+    void applyDynamicFriction(TVStack& r);
+    
     // add dv to velocity
     void updateVelocity(const TVStack& dv);
     
@@ -85,7 +88,6 @@ public:
     void project(TVStack& r)
     {
         BT_PROFILE("project");
-        projection.update();
         projection.project(r);
     }
     
@@ -95,12 +97,6 @@ public:
         m_preconditioner->operator()(x,b);
     }
 
-    virtual void setWorld(btDeformableRigidDynamicsWorld* world)
-    {
-        m_world = world;
-        projection.setWorld(world);
-    }
-    
     virtual void updateId()
     {
         size_t id = 0;

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -1,4 +1,6 @@
 /*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
  Bullet Continuous Collision Detection and Physics Library
  Copyright (c) 2019 Google Inc. http://bulletphysics.org
  This software is provided 'as-is', without any express or implied warranty.
@@ -18,7 +20,7 @@
 
 btDeformableBodySolver::btDeformableBodySolver()
 : m_numNodes(0)
-, m_cg(10)
+, m_cg(50)
 {
     m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
 }
@@ -28,24 +30,19 @@ btDeformableBodySolver::~btDeformableBodySolver()
     delete m_objective;
 }
 
-void btDeformableBodySolver::solveConstraints(float solverdt)
+void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
 {
     BT_PROFILE("solveConstraints");
-    // add constraints to the solver
-    setConstraints();
-    
-    // save v_{n+1}^* velocity after explicit forces
-    backupVelocity();
-    
     m_objective->computeResidual(solverdt, m_residual);
-    
+    m_objective->applyDynamicFriction(m_residual);
     computeStep(m_dv, m_residual);
+    
     updateVelocity();
 }
 
 void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual)
 {
-    btScalar tolerance = std::numeric_limits<float>::epsilon()* 1024 * m_objective->computeNorm(residual);
+    btScalar tolerance = std::numeric_limits<float>::epsilon() * 16 * m_objective->computeNorm(residual);
     m_cg.solve(*m_objective, dv, residual, tolerance);
 }
 
@@ -77,11 +74,16 @@ void btDeformableBodySolver::setConstraints()
     m_objective->setConstraints();
 }
 
-void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world)
+btScalar btDeformableBodySolver::solveContactConstraints()
 {
-    m_objective->setWorld(world);
+    BT_PROFILE("setConstraint");
+    btScalar maxSquaredResidual = m_objective->projection.update();
+    m_objective->enforceConstraint(m_dv);
+    m_objective->updateVelocity(m_dv);
+    return maxSquaredResidual;
 }
 
+
 void btDeformableBodySolver::updateVelocity()
 {
     int counter = 0;
@@ -90,6 +92,11 @@ void btDeformableBodySolver::updateVelocity()
         btSoftBody* psb = m_softBodySet[i];
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
+            // set NaN to zero;
+            if (m_dv[counter] != m_dv[counter])
+            {
+                m_dv[counter].setZero();
+            }
             psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter];
             ++counter;
         }
@@ -136,7 +143,7 @@ bool btDeformableBodySolver::updateNodes()
 }
 
 
-void btDeformableBodySolver::predictMotion(float solverdt)
+void btDeformableBodySolver::predictMotion(btScalar solverdt)
 {
     for (int i = 0; i < m_softBodySet.size(); ++i)
     {

src/BulletSoftBody/btDeformableBodySolver.h

@@ -1,4 +1,6 @@
 /*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
  Bullet Continuous Collision Detection and Physics Library
  Copyright (c) 2019 Google Inc. http://bulletphysics.org
  This software is provided 'as-is', without any express or implied warranty.
@@ -17,13 +19,13 @@
 
 #include "btSoftBodySolvers.h"
 #include "btDeformableBackwardEulerObjective.h"
-#include "btDeformableRigidDynamicsWorld.h"
+#include "btDeformableMultiBodyDynamicsWorld.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 
 struct btCollisionObjectWrapper;
 class btDeformableBackwardEulerObjective;
-class btDeformableRigidDynamicsWorld;
+class btDeformableMultiBodyDynamicsWorld;
 
 class btDeformableBodySolver : public btSoftBodySolver
 {
@@ -37,6 +39,7 @@ protected:
    
     btAlignedObjectArray<btVector3> m_backupVelocity;
     btScalar m_dt;
+    btScalar m_contact_iterations;
     btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;
     
     
@@ -56,9 +59,11 @@ public:
 
     virtual void copyBackToSoftBodies(bool bMove = true) {}
     
-    void extracted(float solverdt);
+    virtual void solveDeformableConstraints(btScalar solverdt);
     
-    virtual void solveConstraints(float solverdt);
+    btScalar solveContactConstraints();
+    
+    virtual void solveConstraints(btScalar dt){}
     
     void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
     
@@ -74,7 +79,7 @@ public:
     
     void computeStep(TVStack& dv, const TVStack& residual);
                      
-    virtual void predictMotion(float solverdt);
+    virtual void predictMotion(btScalar solverdt);
 
     virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
 
@@ -87,8 +92,9 @@ public:
         softBody->defaultCollisionHandler(otherSoftBody);
     }
     virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
+    
     virtual bool checkInitialized(){return true;}
-    virtual void setWorld(btDeformableRigidDynamicsWorld* world);
+
 };
 
 #endif /* btDeformableBodySolver_h */

src/BulletSoftBody/btDeformableContactProjection.cpp

@@ -1,4 +1,6 @@
 /*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
  Bullet Continuous Collision Detection and Physics Library
  Copyright (c) 2019 Google Inc. http://bulletphysics.org
  This software is provided 'as-is', without any express or implied warranty.
@@ -12,214 +14,150 @@
  */
 
 #include "btDeformableContactProjection.h"
-#include "btDeformableRigidDynamicsWorld.h"
+#include "btDeformableMultiBodyDynamicsWorld.h"
 #include <algorithm>
 #include <cmath>
-static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
+btScalar btDeformableContactProjection::update()
-                         btMultiBodyJacobianData& jacobianData,
-                         const btVector3& contact_point,
-                         const btVector3& dir)
 {
-    const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+    btScalar residualSquare = 0;
-    jacobianData.m_jacobians.resize(ndof);
-    jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
-    btScalar* jac = &jacobianData.m_jacobians[0];
-    
-    multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
-    multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
-}
-
-static btVector3 generateUnitOrthogonalVector(const btVector3& u)
-{
-    btScalar ux = u.getX();
-    btScalar uy = u.getY();
-    btScalar uz = u.getZ();
-    btScalar ax = std::abs(ux);
-    btScalar ay = std::abs(uy);
-    btScalar az = std::abs(uz);
-    btVector3 v;
-    if (ax <= ay && ax <= az)
-        v = btVector3(0, -uz, uy);
-    else if (ay <= ax && ay <= az)
-        v = btVector3(-uz, 0, ux);
-    else
-        v = btVector3(-uy, ux, 0);
-    v.normalize();
-    return v;
-}
-
-void btDeformableContactProjection::update()
-{
-    ///solve rigid body constraints
-    m_world->getSolverInfo().m_numIterations = 1;
-    m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo());
-
     // loop through constraints to set constrained values
     for (int index = 0; index < m_constraints.size(); ++index)
     {
-        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
+        DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index);
-        btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
+        const btSoftBody::Node* node = constraint.m_node;
-        for (int i = 0; i < constraints.size(); ++i)
+        for (int j = 0; j < constraint.m_contact.size(); ++j)
         {
-            DeformableContactConstraint& constraint = constraints[i];
+            if (constraint.m_contact[j] == NULL)
-            DeformableFrictionConstraint& friction = frictions[i];
-            for (int j = 0; j < constraint.m_contact.size(); ++j)
             {
-                if (constraint.m_contact[j] == NULL)
+                // nothing needs to be done for dirichelet constraints
+                continue;
+            }
+            const btSoftBody::RContact* c = constraint.m_contact[j];
+            const btSoftBody::sCti& cti = c->m_cti;
+            
+            if (cti.m_colObj->hasContactResponse())
+            {
+                btVector3 va(0, 0, 0);
+                btRigidBody* rigidCol = 0;
+                btMultiBodyLinkCollider* multibodyLinkCol = 0;
+                const btScalar* deltaV_normal;
+                
+                // grab the velocity of the rigid body
+                if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                 {
-                    // nothing needs to be done for dirichelet constraints
+                    rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
-                    continue;
+                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0);
                 }
-                const btSoftBody::RContact* c = constraint.m_contact[j];
+                else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
-                const btSoftBody::sCti& cti = c->m_cti;
-                
-                if (cti.m_colObj->hasContactResponse())
                 {
-                    btVector3 va(0, 0, 0);
+                    multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
-                    btRigidBody* rigidCol = 0;
+                    if (multibodyLinkCol)
-                    btMultiBodyLinkCollider* multibodyLinkCol = 0;
-                    const btScalar* deltaV_normal;
-                    
-                    // grab the velocity of the rigid body
-                    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                     {
-                        rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+                        const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-                        va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0);
+                        const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
-                    }
+                        const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
-                    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                        const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
-                    {
+                        const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
-                        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                        const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
-                        if (multibodyLinkCol)
+                        deltaV_normal = &c->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+                        // add in the normal component of the va
+                        btScalar vel = 0.0;
+                        for (int k = 0; k < ndof; ++k)
                         {
-                            const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                            vel += (local_v[k]+local_dv[k]) * J_n[k];
-                            const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
-                            const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
-                            const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
-                            const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
-                            deltaV_normal = &c->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
-                            // add in the normal component of the va
-                            btScalar vel = 0.0;
-                            for (int k = 0; k < ndof; ++k)
-                            {
-                                vel += local_v[k] * J_n[k];
-                            }
-                            va = cti.m_normal * vel * m_dt;
-
-                            // add in the tangential components of the va
-                            vel = 0.0;
-                            for (int k = 0; k < ndof; ++k)
-                            {
-                                vel += local_v[k] * J_t1[k];
-                            }
-                            va += c->t1 * vel * m_dt;
-                            vel = 0.0;
-                            for (int k = 0; k < ndof; ++k)
-                            {
-                                vel += local_v[k] * J_t2[k];
-                            }
-                            va += c->t2 * vel * m_dt;
                         }
-                    }
+                        va = cti.m_normal * vel * m_dt;
-                    
+                        // add in the tangential components of the va
-                    const btVector3 vb = c->m_node->m_v * m_dt;
+                        vel = 0.0;
-                    const btVector3 vr = vb - va;
+                        for (int k = 0; k < ndof; ++k)
-                    const btScalar dn = btDot(vr, cti.m_normal);
-                    btVector3 impulse = c->m_c0 * vr;
-                    const btVector3 impulse_normal = c->m_c0 * (cti.m_normal * dn);
-                    const btVector3 impulse_tangent = impulse - impulse_normal;
-                    
-                    // start friction handling
-                    // copy old data
-                    friction.m_impulse_prev[j] = friction.m_impulse[j];
-                    friction.m_dv_prev[j] = friction.m_dv[j];
-                    friction.m_static_prev[j] = friction.m_static[j];
-                    
-                    // get the current tangent direction
-                    btScalar local_tangent_norm = impulse_tangent.norm();
-                    btVector3 local_tangent_dir = btVector3(0,0,0);
-                    if (local_tangent_norm > SIMD_EPSILON)
-                        local_tangent_dir = impulse_tangent.normalized();
-
-                    // accumulated impulse on the rb in this and all prev cg iterations
-                    constraint.m_accumulated_normal_impulse[j] += impulse_normal.dot(cti.m_normal);
-                    const btScalar& accumulated_normal = constraint.m_accumulated_normal_impulse[j];
-                    
-                    // the total tangential impulse required to stop sliding
-                    btVector3 tangent = friction.m_accumulated_tangent_impulse[j] + impulse_tangent;
-                    btScalar tangent_norm = tangent.norm();
-  
-                    if (accumulated_normal < 0)
-                    {
-                        friction.m_direction[j] = -local_tangent_dir;
-                        // do not allow switching from static friction to dynamic friction
-                        // it causes cg to explode
-                        if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false)
                         {
-                            friction.m_static[j] = false;
+                            vel += (local_v[k]+local_dv[k]) * J_t1[k];
-                            friction.m_impulse[j] = -accumulated_normal*c->m_c3;
+                        }
+                        va += c->t1 * vel * m_dt;
+                        vel = 0.0;
+                        for (int k = 0; k < ndof; ++k)
+                        {
+                            vel += (local_v[k]+local_dv[k]) * J_t2[k];
+                        }
+                        va += c->t2 * vel * m_dt;
+                    }
+                }
+                
+                const btVector3 vb = c->m_node->m_v * m_dt;
+                const btVector3 vr = vb - va;
+                const btScalar dn = btDot(vr, cti.m_normal);
+                btVector3 impulse = c->m_c0 * vr;
+                const btVector3 impulse_normal = c->m_c0 * (cti.m_normal * dn);
+                btVector3 impulse_tangent = impulse - impulse_normal;
+                
+                btVector3 old_total_tangent_dv = constraint.m_total_tangent_dv[j];
+                constraint.m_total_normal_dv[j] -= impulse_normal * node->m_im;
+                constraint.m_total_tangent_dv[j] -= impulse_tangent * node->m_im;
+                
+                if (constraint.m_total_normal_dv[j].dot(cti.m_normal) < 0)
+                {
+                    // separating in the normal direction
+                    constraint.m_static[j] = false;
+                    constraint.m_can_be_dynamic[j] = false;
+                    constraint.m_total_tangent_dv[j] = btVector3(0,0,0);
+                    impulse_tangent.setZero();
+                }
+                else
+                {
+                    if (constraint.m_can_be_dynamic[j] && constraint.m_total_normal_dv[j].norm() * c->m_c3 < constraint.m_total_tangent_dv[j].norm())
+                    {
+                        // dynamic friction
+                        // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
+                        constraint.m_static[j] = false;
+                        constraint.m_can_be_dynamic[j] = true;
+                        if (constraint.m_total_tangent_dv[j].norm() < SIMD_EPSILON)
+                        {
+                            constraint.m_total_tangent_dv[j] = btVector3(0,0,0);
                         }
                         else
                         {
-                            friction.m_static[j] = true;
+                            constraint.m_total_tangent_dv[j] = constraint.m_total_tangent_dv[j].normalized() * constraint.m_total_normal_dv[j].norm() * c->m_c3;
-                            friction.m_impulse[j] = tangent_norm;
                         }
+                        impulse_tangent = -btScalar(1)/node->m_im * (constraint.m_total_tangent_dv[j] - old_total_tangent_dv);
                     }
                     else
                     {
-                        friction.m_released[j] = true;
+                        // static friction
-                        friction.m_static[j] = false;
+                        constraint.m_static[j] = true;
-                        friction.m_impulse[j] = 0;
+                        constraint.m_can_be_dynamic[j] = false;
-                        friction.m_direction[j] = btVector3(0,0,0);
                     }
-                    friction.m_dv[j] = friction.m_impulse[j] * c->m_c2/m_dt;
+                }
-                    friction.m_accumulated_tangent_impulse[j] = -friction.m_impulse[j] * friction.m_direction[j];
+                impulse = impulse_normal + impulse_tangent;
                 
-                    // the incremental impulse applied to rb in the tangential direction
+                // dn is the normal component of velocity diffrerence. Approximates the residual.
-                    btVector3 incremental_tangent = (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
+                residualSquare = btMax(residualSquare, dn*dn);
-                    
+                if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
-
+                {
-                    // dv = new_impulse + accumulated velocity change in previous CG iterations
+                    if (rigidCol)
-                    // so we have the invariant node->m_v = backupVelocity + dv;
-
-                    btScalar dvn = -accumulated_normal * c->m_c2/m_dt;
-                    
-                    // the following is equivalent
-                    /*
-                        btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices->at(c->m_node)];
-                        btScalar dvn = dv.dot(cti.m_normal);
-                     */
-                    
-                    constraint.m_value[j] = dvn;
-                    
-                    // the incremental impulse:
-                    // in the normal direction it's the normal component of "impulse"
-                    // in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration
-                    impulse = impulse_normal + incremental_tangent;
-                    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                     {
-                        if (rigidCol)
+                        rigidCol->applyImpulse(impulse, c->m_c1);
-                            rigidCol->applyImpulse(impulse, c->m_c1);
                     }
-                    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                }
+                else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                {
+                    if (multibodyLinkCol)
                     {
-                        if (multibodyLinkCol)
+                        // apply normal component of the impulse
+                        multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
+                        if (impulse_tangent.norm() > SIMD_EPSILON)
                         {
-                            // apply normal component of the impulse
+                            // apply tangential component of the impulse
-                            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
+                            const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
-                            if (incremental_tangent.norm() > SIMD_EPSILON)
+                            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(c->t1));
-                            {
+                            const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
-                                // apply tangential component of the impulse
+                            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(c->t2));
-                               const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
-                                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1));
-                                const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
-                                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t2, impulse.dot(c->t2));
-                            }
                         }
                     }
                 }
             }
         }
     }
+    return residualSquare;
 }
 
 void btDeformableContactProjection::setConstraints()
@@ -233,19 +171,7 @@ void btDeformableContactProjection::setConstraints()
         {
             if (psb->m_nodes[j].m_im == 0)
             {
-                btAlignedObjectArray<DeformableContactConstraint> c;
+                m_constraints.insert(psb->m_nodes[j].index, DeformableContactConstraint());
-                c.reserve(3);
-                c.push_back(DeformableContactConstraint(btVector3(1,0,0)));
-                c.push_back(DeformableContactConstraint(btVector3(0,1,0)));
-                c.push_back(DeformableContactConstraint(btVector3(0,0,1)));
-                m_constraints.insert(psb->m_nodes[j].index, c);
-                
-                btAlignedObjectArray<DeformableFrictionConstraint> f;
-                f.reserve(3);
-                f.push_back(DeformableFrictionConstraint());
-                f.push_back(DeformableFrictionConstraint());
-                f.push_back(DeformableFrictionConstraint());
-                m_frictions.insert(psb->m_nodes[j].index, f);
             }
         }
     }
@@ -300,41 +226,12 @@ void btDeformableContactProjection::setConstraints()
                     
                     if (m_constraints.find(c.m_node->index) == NULL)
                     {
-                        btAlignedObjectArray<DeformableContactConstraint> constraints;
+                        m_constraints.insert(c.m_node->index, DeformableContactConstraint(c));
-                        constraints.push_back(DeformableContactConstraint(c));
-                        m_constraints.insert(c.m_node->index,constraints);
-                        btAlignedObjectArray<DeformableFrictionConstraint> frictions;
-                        frictions.push_back(DeformableFrictionConstraint());
-                        m_frictions.insert(c.m_node->index,frictions);
                     }
                     else
                     {
-                        // group colinear constraints into one
+                        DeformableContactConstraint& constraints = *m_constraints[c.m_node->index];
-                        const btScalar angle_epsilon = 0.015192247; // less than 10 degree
+                        constraints.append(c);
-                        bool merged = false;
-                        btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints[c.m_node->index];
-                        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[c.m_node->index];
-                        for (int j = 0; j < constraints.size(); ++j)
-                        {
-                            const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction;
-                            btScalar dot_prod = dirs[0].dot(cti.m_normal);
-                            if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon)
-                            {
-                                // group the constraints
-                                constraints[j].append(c);
-                                // push in an empty friction
-                                frictions[j].append();
-                                merged = true;
-                                break;
-                            }
-                        }
-                        const int dim = 3;
-                        // hard coded no more than 3 constraint directions
-                        if (!merged && constraints.size() < dim)
-                        {
-                            constraints.push_back(DeformableContactConstraint(c));
-                            frictions.push_back(DeformableFrictionConstraint());
-                        }
                     }
                 }
             }
@@ -344,62 +241,15 @@ void btDeformableContactProjection::setConstraints()
 
 void btDeformableContactProjection::enforceConstraint(TVStack& x)
 {
-    const int dim = 3;
     for (int index = 0; index < m_constraints.size(); ++index)
     {
-        const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
+        const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index);
         size_t i = m_constraints.getKeyAtIndex(index).getUid1();
-        const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
+        x[i].setZero();
-        btAssert(constraints.size() <= dim);
+        for (int j = 0; j < constraints.m_total_normal_dv.size(); ++j)
-        btAssert(constraints.size() > 0);
-        if (constraints.size() == 1)
         {
-            x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
+            x[i] += constraints.m_total_normal_dv[j];
-            for (int j = 0; j < constraints[0].m_direction.size(); ++j)
+            x[i] += constraints.m_total_tangent_dv[j];
-                x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j];
-        }
-        else if (constraints.size() == 2)
-        {
-            btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
-            btAssert(free_dir.norm() > SIMD_EPSILON)
-            free_dir.normalize();
-            x[i] = x[i].dot(free_dir) * free_dir;
-            for (int j = 0; j < constraints.size(); ++j)
-            {
-                for (int k = 0; k < constraints[j].m_direction.size(); ++k)
-                {
-                    x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
-                }
-            }
-        }
-        else
-        {
-            x[i].setZero();
-            for (int j = 0; j < constraints.size(); ++j)
-            {
-                for (int k = 0; k < constraints[j].m_direction.size(); ++k)
-                {
-                    x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
-                }
-            }
-        }
-        
-        // apply friction if the node is not constrained in all directions
-        if (constraints.size() < 3)
-        {
-            for (int f = 0; f < frictions.size(); ++f)
-            {
-                const DeformableFrictionConstraint& friction= frictions[f];
-                for (int j = 0; j < friction.m_direction.size(); ++j)
-                {
-                    // add the friction constraint
-                    if (friction.m_static[j] == true)
-                    {
-                        x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j];
-                        x[i] += friction.m_direction[j] * friction.m_dv[j];
-                    }
-                }
-            }
         }
     }
 }
@@ -409,59 +259,80 @@ void btDeformableContactProjection::project(TVStack& x)
     const int dim = 3;
     for (int index = 0; index < m_constraints.size(); ++index)
     {
-        const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
+        const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index);
         size_t i = m_constraints.getKeyAtIndex(index).getUid1();
-        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
+        if (constraints.m_contact[0] == NULL)
-        btAssert(constraints.size() <= dim);
-        btAssert(constraints.size() > 0);
-        if (constraints.size() == 1)
         {
-            x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
+            // static node
+            x[i].setZero();
+            continue;
         }
-        else if (constraints.size() == 2)
+        bool has_static = false;
+        for (int j = 0; j < constraints.m_static.size(); ++j)
         {
-            btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
+            has_static = has_static || constraints.m_static[j];
-            btAssert(free_dir.norm() > SIMD_EPSILON)
+        }
-            free_dir.normalize();
+        // static friction => fully constrained
-            x[i] = x[i].dot(free_dir) * free_dir;
+        if (has_static)
+        {
+            x[i].setZero();
+        }
+        else if (constraints.m_total_normal_dv.size() >= dim)
+        {
+            x[i].setZero();
+        }
+        else if (constraints.m_total_normal_dv.size() == 2)
+        {
+            
+            btVector3 dir0 = (constraints.m_total_normal_dv[0].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[0].normalized() : btVector3(0,0,0);
+            btVector3 dir1 = (constraints.m_total_normal_dv[1].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[1].normalized() : btVector3(0,0,0);
+            btVector3 free_dir = btCross(dir0, dir1);
+            if (free_dir.norm() < SIMD_EPSILON)
+            {
+                x[i] -= x[i].dot(dir0) * dir0;
+                x[i] -= x[i].dot(dir1) * dir1;
+            }
+            else
+            {
+                free_dir.normalize();
+                x[i] = x[i].dot(free_dir) * free_dir;
+            }
         }
         else
-            x[i].setZero();
-        
-        // apply friction if the node is not constrained in all directions
-        if (constraints.size() < 3)
         {
-            bool has_static_constraint = false;
+            btAssert(constraints.m_total_normal_dv.size() == 1);
-            for (int f = 0; f < frictions.size(); ++f)
+            btVector3 dir0 = (constraints.m_total_normal_dv[0].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[0].normalized() : btVector3(0,0,0);
+            x[i] -= x[i].dot(dir0) * dir0;
+        }
+    }
+}
+
+void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
+{
+    for (int index = 0; index < m_constraints.size(); ++index)
+    {
+        const DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index);
+        const btSoftBody::Node* node = constraint.m_node;
+        if (node == NULL)
+            continue;
+        size_t i = m_constraints.getKeyAtIndex(index).getUid1();
+        bool has_static_constraint = false;
+        
+        // apply dynamic friction force (scaled by dt) if the node does not have static friction constraint
+        for (int j = 0; j < constraint.m_static.size(); ++j)
+        {
+            if (constraint.m_static[j])
             {
-                DeformableFrictionConstraint& friction= frictions[f];
+                has_static_constraint = true;
-                for (int j = 0; j < friction.m_static.size(); ++j)
+                break;
-                    has_static_constraint = has_static_constraint || friction.m_static[j];
             }
-            
+        }
-            for (int f = 0; f < frictions.size(); ++f)
+        for (int j = 0; j < constraint.m_total_tangent_dv.size(); ++j)
+        {
+            btVector3 friction_force =  constraint.m_total_tangent_dv[j] * (1./node->m_im);
+            if (!has_static_constraint)
             {
-                DeformableFrictionConstraint& friction= frictions[f];
+                f[i] += friction_force;
-                for (int j = 0; j < friction.m_direction.size(); ++j)
-                {
-                    // clear the old friction force
-                    if (friction.m_static_prev[j] == false)
-                    {
-                        x[i] -= friction.m_direction_prev[j] * friction.m_impulse_prev[j];
-                    }
-                    
-                    // only add to the rhs if there is no static friction constraint on the node
-                    if (friction.m_static[j] == false)
-                    {
-                        if (!has_static_constraint)
-                            x[i] += friction.m_direction[j] * friction.m_impulse[j];
-                    }
-                    else
-                    {
-                        // otherwise clear the constraint in the friction direction
-                        x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j];
-                    }
-                }
             }
         }
     }
@@ -471,7 +342,6 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 {
     btCGProjection::reinitialize(nodeUpdated);
     m_constraints.clear();
-    m_frictions.clear();
 }
 
 

src/BulletSoftBody/btDeformableContactProjection.h

@@ -1,4 +1,6 @@
 /*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
  Bullet Continuous Collision Detection and Physics Library
  Copyright (c) 2019 Google Inc. http://bulletphysics.org
  This software is provided 'as-is', without any express or implied warranty.
@@ -22,8 +24,7 @@ class btDeformableContactProjection : public btCGProjection
 {
 public:
     // map from node index to constraints
-    btHashMap<btHashInt, btAlignedObjectArray<DeformableContactConstraint> > m_constraints;
+    btHashMap<btHashInt, DeformableContactConstraint> m_constraints;
-    btHashMap<btHashInt, btAlignedObjectArray<DeformableFrictionConstraint> >m_frictions;
     
     btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
     : btCGProjection(softBodies, dt)
@@ -36,12 +37,14 @@ public:
     
     // apply the constraints to the rhs
     virtual void project(TVStack& x);
+    // add to friction
+    virtual void applyDynamicFriction(TVStack& f);
     
     // apply constraints to x in Ax=b
     virtual void enforceConstraint(TVStack& x);
     
     // update the constraints
-    virtual void update();
+    virtual btScalar update();
     
     virtual void setConstraints();
     

src/BulletSoftBody/btDeformableCorotatedForce.h

@@ -0,0 +1,120 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_COROTATED_H
+#define BT_COROTATED_H
+
+#include "btDeformableLagrangianForce.h"
+#include "LinearMath/btPolarDecomposition.h"
+
+static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
+{
+    static const btPolarDecomposition polar;
+    return polar.decompose(m, q, s);
+}
+
+class btDeformableCorotatedForce : public btDeformableLagrangianForce
+{
+public:
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    btScalar m_mu, m_lambda;
+    btDeformableCorotatedForce(): m_mu(1), m_lambda(1)
+    {
+        
+    }
+    
+    btDeformableCorotatedForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda)
+    {
+    }
+    
+    virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
+    {
+    }
+    
+    virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+    {
+        addScaledElasticForce(scale, force);
+    }
+    
+    virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+    {
+    }
+    
+    virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+    {
+        int numNodes = getNumNodes();
+        btAssert(numNodes <= force.size())
+        btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_tetras.size(); ++j)
+            {
+                btSoftBody::Tetra& tetra = psb->m_tetras[j];
+                btMatrix3x3 P;
+                firstPiola(tetra.m_F,P);
+                btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+                btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
+                
+                btSoftBody::Node* node0 = tetra.m_n[0];
+                btSoftBody::Node* node1 = tetra.m_n[1];
+                btSoftBody::Node* node2 = tetra.m_n[2];
+                btSoftBody::Node* node3 = tetra.m_n[3];
+                size_t id0 = node0->index;
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                size_t id3 = node3->index;
+                
+                // elastic force
+                // explicit elastic force
+                btScalar scale1 = scale * tetra.m_element_measure;
+                force[id0] -= scale1 * force_on_node0;
+                force[id1] -= scale1 * force_on_node123.getColumn(0);
+                force[id2] -= scale1 * force_on_node123.getColumn(1);
+                force[id3] -= scale1 * force_on_node123.getColumn(2);
+            }
+        }
+    }
+    
+    void firstPiola(const btMatrix3x3& F, btMatrix3x3& P)
+    {
+        // btMatrix3x3 JFinvT = F.adjoint();
+        btScalar J = F.determinant();
+        P =  F.adjoint() * (m_lambda * (J-1));
+        if (m_mu > SIMD_EPSILON)
+        {
+            btMatrix3x3 R,S;
+            if (J < 1024 * SIMD_EPSILON)
+                R.setIdentity();
+            else
+                PolarDecompose(F, R, S); // this QR is not robust, consider using implicit shift svd
+            /*https://fuchuyuan.github.io/research/svd/paper.pdf*/
+            P += (F-R) * 2 * m_mu;
+        }
+    }
+    
+    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+    {
+    }
+    
+    virtual btDeformableLagrangianForceType getForceType()
+    {
+        return BT_COROTATED_FORCE;
+    }
+    
+};
+
+
+#endif /* btCorotated_h */

src/BulletSoftBody/btDeformableGravityForce.h

@@ -1,4 +1,6 @@
 /*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
  Bullet Continuous Collision Detection and Physics Library
  Copyright (c) 2019 Google Inc. http://bulletphysics.org
  This software is provided 'as-is', without any express or implied warranty.

src/BulletSoftBody/btDeformableLagrangianForce.h

@@ -1,4 +1,6 @@
 /*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
  Bullet Continuous Collision Detection and Physics Library
  Copyright (c) 2019 Google Inc. http://bulletphysics.org
  This software is provided 'as-is', without any express or implied warranty.
@@ -20,13 +22,14 @@
 enum btDeformableLagrangianForceType
 {
     BT_GRAVITY_FORCE = 1,
-    BT_MASSSPRING_FORCE = 2
+    BT_MASSSPRING_FORCE = 2,
+    BT_COROTATED_FORCE = 3,
+    BT_NEOHOOKEAN_FORCE = 4
 };
 
 class btDeformableLagrangianForce
 {
 public:
-//    using TVStack = btAlignedObjectArray<btVector3>;
     typedef btAlignedObjectArray<btVector3> TVStack;
     btAlignedObjectArray<btSoftBody *> m_softBodies;
     const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;

src/BulletSoftBody/btDeformableMassSpringForce.h

@@ -1,4 +1,6 @@
 /*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
  Bullet Continuous Collision Detection and Physics Library
  Copyright (c) 2019 Google Inc. http://bulletphysics.org
  This software is provided 'as-is', without any express or implied warranty.
@@ -18,10 +20,14 @@
 
 class btDeformableMassSpringForce : public btDeformableLagrangianForce
 {
+    bool m_momentum_conserving;
+    btScalar m_elasticStiffness, m_dampingStiffness;
 public:
-//    using TVStack = btDeformableLagrangianForce::TVStack;
     typedef btAlignedObjectArray<btVector3> TVStack;
-    btDeformableMassSpringForce()
+    btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05)
+    {
+    }
+    btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d)
     {
     }
     
@@ -52,8 +58,15 @@ public:
                 
                 // damping force
                 btVector3 v_diff = (node2->m_v - node1->m_v);
-                btScalar k_damp = psb->m_dampingCoefficient;
+                btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
-                btVector3 scaled_force = scale * v_diff * k_damp;
+                if (m_momentum_conserving)
+                {
+                    if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON)
+                    {
+                        btVector3 dir = (node2->m_x - node1->m_x).normalized();
+                        scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
+                    }
+                }
                 force[id1] += scaled_force;
                 force[id2] -= scaled_force;
             }
@@ -72,7 +85,6 @@ public:
                 const btSoftBody::Link& link = psb->m_links[j];
                 btSoftBody::Node* node1 = link.m_n[0];
                 btSoftBody::Node* node2 = link.m_n[1];
-                btScalar kLST = link.Feature::m_material->m_kLST;
                 btScalar r = link.m_rl;
                 size_t id1 = node1->index;
                 size_t id2 = node2->index;
@@ -80,8 +92,8 @@ public:
                 // elastic force
                 // explicit elastic force
                 btVector3 dir = (node2->m_q - node1->m_q);
-                btVector3 dir_normalized = dir.normalized();
+                btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
-                btVector3 scaled_force = scale * kLST * (dir - dir_normalized * r);
+                btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r);
                 force[id1] += scaled_force;
                 force[id2] -= scaled_force;
             }
@@ -94,7 +106,7 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             const btSoftBody* psb = m_softBodies[i];
-            btScalar scaled_k_damp = psb->m_dampingCoefficient * scale;
+            btScalar scaled_k_damp = m_dampingStiffness * scale;
             for (int j = 0; j < psb->m_links.size(); ++j)
             {
                 const btSoftBody::Link& link = psb->m_links[j];
@@ -102,7 +114,16 @@ public:
                 btSoftBody::Node* node2 = link.m_n[1];
                 size_t id1 = node1->index;
                 size_t id2 = node2->index;
+
                 btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
+                if (m_momentum_conserving)
+                {
+                    if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON)
+                    {
+                        btVector3 dir = (node2->m_x - node1->m_x).normalized();
+                        local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
+                    }
+                }
                 df[id1] += local_scaled_df;
                 df[id2] -= local_scaled_df;
             }

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -0,0 +1,53 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+
+#include "btDeformableMultiBodyConstraintSolver.h"
+#include <iostream>
+// override the iterations method to include deformable/multibody contact
+btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+    {
+        ///this is a special step to resolve penetrations (just for contacts)
+        solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+        
+        int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
+        for (int iteration = 0; iteration < maxIterations; iteration++)
+        {
+            m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+            
+            solverBodyWriteBack(infoGlobal);
+            m_leastSquaresResidual = btMax(m_leastSquaresResidual, m_deformableSolver->solveContactConstraints());
+            writeToSolverBody(bodies, numBodies, infoGlobal);
+            
+            if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
+            {
+#ifdef VERBOSE_RESIDUAL_PRINTF
+                printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
+#endif
+                m_analyticsData.m_numSolverCalls++;
+                m_analyticsData.m_numIterationsUsed = iteration+1;
+                m_analyticsData.m_islandId = -2;
+                if (numBodies>0)
+                    m_analyticsData.m_islandId = bodies[0]->getCompanionId();
+                m_analyticsData.m_numBodies = numBodies;
+                m_analyticsData.m_numContactManifolds = numManifolds;
+                m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
+                break;
+            }
+        }
+    }
+    return 0.f;
+}

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h

@@ -0,0 +1,107 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+
+#ifndef BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
+#define BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
+
+#include "btDeformableBodySolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+
+class btDeformableBodySolver;
+
+ATTRIBUTE_ALIGNED16(class)
+btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver
+{
+    btDeformableBodySolver* m_deformableSolver;
+    
+protected:
+    // override the iterations method to include deformable/multibody contact
+    virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+    
+    void solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
+    {
+        for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
+        {
+            btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
+            if (body)
+            {
+                m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
+                m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
+            }
+        }
+    }
+    
+    void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+    {
+        btSISolverSingleIterationData siData(m_tmpSolverBodyPool,
+                                             m_tmpSolverContactConstraintPool,
+                                             m_tmpSolverNonContactConstraintPool,
+                                             m_tmpSolverContactFrictionConstraintPool,
+                                             m_tmpSolverContactRollingFrictionConstraintPool,
+                                             m_orderTmpConstraintPool,
+                                             m_orderNonContactConstraintPool,
+                                             m_orderFrictionConstraintPool,
+                                             m_tmpConstraintSizesPool,
+                                             m_resolveSingleConstraintRowGeneric,
+                                             m_resolveSingleConstraintRowLowerLimit,
+                                             m_resolveSplitPenetrationImpulse,
+                                             m_kinematicBodyUniqueIdToSolverBodyTable,
+                                             m_btSeed2,
+                                             m_fixedBodyId,
+                                             m_maxOverrideNumSolverIterations);
+        
+        for (int i = 0; i < numBodies; i++)
+        {
+            int bodyId = siData.getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
+            
+            btRigidBody* body = btRigidBody::upcast(bodies[i]);
+            if (body && body->getInvMass())
+            {
+                btSolverBody& solverBody = siData.m_tmpSolverBodyPool[bodyId];
+                solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
+                solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
+            }
+        }
+    }
+    
+public:
+    BT_DECLARE_ALIGNED_ALLOCATOR();
+    
+    void setDeformableSolver(btDeformableBodySolver* deformableSolver)
+    {
+        m_deformableSolver = deformableSolver;
+    }
+    
+    virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+    {
+        m_tmpMultiBodyConstraints = multiBodyConstraints;
+        m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
+        
+        // inherited from MultiBodyConstraintSolver
+        solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+        
+        // overriden
+        solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+        
+        // inherited from MultiBodyConstraintSolver
+        solveGroupCacheFriendlyFinish(bodies, numBodies, info);
+        
+        m_tmpMultiBodyConstraints = 0;
+        m_tmpNumMultiBodyConstraints = 0;
+    }
+};
+
+#endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -0,0 +1,426 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+/* ====== Overview of the Deformable Algorithm ====== */
+
+/*
+A single step of the deformable body simulation contains the following main components:
+1. Update velocity to a temporary state v_{n+1}^* = v_n + explicit_force * dt / mass, where explicit forces include gravity and elastic forces.
+2. Detect collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*.
+3. Then velocities of deformable bodies v_{n+1} are solved in
+        M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass,
+   by a conjugate gradient solver, where the damping force is implicit and depends on v_{n+1}.
+4. Contact constraints are solved as projections as in the paper by Baraff and Witkin https://www.cs.cmu.edu/~baraff/papers/sig98.pdf. Dynamic frictions are treated as a force and added to the rhs of the CG solve, whereas static frictions are treated as constraints similar to contact.
+5. Position is updated via x_{n+1} = x_n + dt * v_{n+1}.
+6. Apply position correction to prevent numerical drift.
+
+The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf
+ */
+
+#include <stdio.h>
+#include "btDeformableMultiBodyDynamicsWorld.h"
+#include "btDeformableBodySolver.h"
+#include "LinearMath/btQuickprof.h"
+
+void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
+{
+    BT_PROFILE("internalSingleStepSimulation");
+    reinitialize(timeStep);
+    // add gravity to velocity of rigid and multi bodys
+    applyRigidBodyGravity(timeStep);
+    
+    ///apply gravity and explicit force to velocity, predict motion
+    predictUnconstraintMotion(timeStep);
+    
+    ///perform collision detection
+    btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
+    
+    btMultiBodyDynamicsWorld::calculateSimulationIslands();
+    
+    beforeSolverCallbacks(timeStep);
+    
+    ///solve deformable bodies constraints
+    solveConstraints(timeStep);
+    
+    afterSolverCallbacks(timeStep);
+    
+    integrateTransforms(timeStep);
+    
+    ///update vehicle simulation
+    btMultiBodyDynamicsWorld::updateActions(timeStep);
+    
+    btMultiBodyDynamicsWorld::updateActivationState(timeStep);
+    // End solver-wise simulation step
+    // ///////////////////////////////
+}
+
+void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep)
+{
+    // perform position correction for all constraints 
+    BT_PROFILE("positionCorrection");
+    for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index)
+    {
+        DeformableContactConstraint& constraint = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index);
+        for (int j = 0; j < constraint.m_contact.size(); ++j)
+        {
+            const btSoftBody::RContact* c = constraint.m_contact[j];
+            // skip anchor points
+            if (c == NULL || c->m_node->m_im == 0)
+                continue;
+            const btSoftBody::sCti& cti = c->m_cti;
+            btVector3 va(0, 0, 0);
+            
+            // grab the velocity of the rigid body
+            if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+            {
+                btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+                va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0);
+            }
+            else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+            {
+                btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                if (multibodyLinkCol)
+                {
+                    const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                    const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
+                    const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
+                    const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
+                    const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
+                    // add in the normal component of the va
+                    btScalar vel = 0.0;
+                    for (int k = 0; k < ndof; ++k)
+                    {
+                        vel += local_v[k] * J_n[k];
+                    }
+                    va = cti.m_normal * vel;
+                    
+                    vel = 0.0;
+                    for (int k = 0; k < ndof; ++k)
+                    {
+                        vel += local_v[k] * J_t1[k];
+                    }
+                    va += c->t1 * vel;
+                    vel = 0.0;
+                    for (int k = 0; k < ndof; ++k)
+                    {
+                        vel += local_v[k] * J_t2[k];
+                    }
+                    va += c->t2 * vel;
+                }
+            }
+            else
+            {
+                // The object interacting with deformable node is not supported for position correction
+                btAssert(false);
+            }
+            
+            if (cti.m_colObj->hasContactResponse())
+            {
+                btScalar dp = cti.m_offset;
+                
+                // only perform position correction when penetrating
+                if (dp < 0)
+                {
+                    c->m_node->m_v -= dp * cti.m_normal / timeStep;
+                }
+            }
+        }
+    }
+}
+
+
+void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
+{
+    BT_PROFILE("integrateTransforms");
+    m_deformableBodySolver->backupVelocity();
+    positionCorrection(timeStep);
+    btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            btSoftBody::Node& node = psb->m_nodes[j];
+            btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
+            btScalar clampDeltaV = maxDisplacement / timeStep;
+            for (int c = 0; c < 3; c++)
+            {
+                if (node.m_v[c] > clampDeltaV)
+                {
+                    node.m_v[c] = clampDeltaV;
+                }
+                if (node.m_v[c] < -clampDeltaV)
+                {
+                    node.m_v[c] = -clampDeltaV;
+                }
+            }
+            node.m_x  =  node.m_q + timeStep * node.m_v;
+        }
+    }
+    m_deformableBodySolver->revertVelocity();
+}
+
+void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
+{
+    // save v_{n+1}^* velocity after explicit forces
+    m_deformableBodySolver->backupVelocity();
+    
+    // set up constraints among multibodies and between multibodies and deformable bodies
+    setupConstraints();
+    solveMultiBodyRelatedConstraints();
+    m_deformableBodySolver->solveDeformableConstraints(timeStep);
+}
+
+void btDeformableMultiBodyDynamicsWorld::setupConstraints()
+{
+    // set up constraints between multibody and deformable bodies
+    m_deformableBodySolver->setConstraints();
+    
+    // set up constraints among multibodies
+    {
+        sortConstraints();
+        // setup the solver callback
+        btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
+        btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
+        m_solverMultiBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
+        m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
+        
+        // build islands
+        m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
+        // write the constraint information of each island to the callback, and also setup the constraints in solver
+    }
+}
+
+void btDeformableMultiBodyDynamicsWorld::sortConstraints()
+{
+    m_sortedConstraints.resize(m_constraints.size());
+    int i;
+    for (i = 0; i < getNumConstraints(); i++)
+    {
+        m_sortedConstraints[i] = m_constraints[i];
+    }
+    m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
+    
+    m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
+    for (i = 0; i < m_multiBodyConstraints.size(); i++)
+    {
+        m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
+    }
+    m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
+}
+    
+    
+void btDeformableMultiBodyDynamicsWorld::solveMultiBodyRelatedConstraints()
+{
+    // process constraints on each island
+    m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
+    
+    // process deferred
+    m_solverMultiBodyIslandCallback->processConstraints();
+    m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer);
+    
+    // write joint feedback
+    {
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
+            
+            bool isSleeping = false;
+            
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
+            
+            if (!isSleeping)
+            {
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
+                m_scratch_m.resize(bod->getNumLinks() + 1);
+                
+                if (bod->internalNeedsJointFeedback())
+                {
+                    if (!bod->isUsingRK4Integration())
+                    {
+                        if (bod->internalNeedsJointFeedback())
+                        {
+                            bool isConstraintPass = true;
+                            bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+                                                                                      getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                      getSolverInfo().m_jointFeedbackInJointFrame);
+                        }
+                    }
+                }
+            }
+        }
+    }
+    
+    // todo : may not be necessary
+    for (int i = 0; i < this->m_multiBodies.size(); i++)
+    {
+        btMultiBody* bod = m_multiBodies[i];
+        bod->processDeltaVeeMultiDof2();
+    }
+}
+
+void btDeformableMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask)
+{
+    m_softBodies.push_back(body);
+    
+    // Set the soft body solver that will deal with this body
+    // to be the world's solver
+    body->setSoftBodySolver(m_deformableBodySolver);
+    
+    btCollisionWorld::addCollisionObject(body,
+                                         collisionFilterGroup,
+                                         collisionFilterMask);
+}
+
+void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+    BT_PROFILE("predictUnconstraintMotion");
+    btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep);
+    m_deformableBodySolver->predictMotion(timeStep);
+}
+
+void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
+{
+    m_internalTime += timeStep;
+    m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
+    btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
+    dispatchInfo.m_timeStep = timeStep;
+    dispatchInfo.m_stepCount = 0;
+    dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
+    btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
+}
+
+void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep)
+{
+    // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
+    // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
+    // when there are multiple substeps
+    clearForces();
+    clearMultiBodyForces();
+    btMultiBodyDynamicsWorld::applyGravity();
+    // integrate rigid body gravity
+    for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
+    {
+        btRigidBody* rb = m_nonStaticRigidBodies[i];
+        rb->integrateVelocities(timeStep);
+    }
+    
+    // integrate multibody gravity
+    {
+        forwardKinematics();
+        clearMultiBodyConstraintForces();
+        {
+            for (int i = 0; i < this->m_multiBodies.size(); i++)
+            {
+                btMultiBody* bod = m_multiBodies[i];
+                
+                bool isSleeping = false;
+                
+                if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+                {
+                    isSleeping = true;
+                }
+                for (int b = 0; b < bod->getNumLinks(); b++)
+                {
+                    if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                        isSleeping = true;
+                }
+                
+                if (!isSleeping)
+                {
+                    m_scratch_r.resize(bod->getNumLinks() + 1);
+                    m_scratch_v.resize(bod->getNumLinks() + 1);
+                    m_scratch_m.resize(bod->getNumLinks() + 1);
+                    bool isConstraintPass = false;
+                    {
+                        if (!bod->isUsingRK4Integration())
+                        {
+                            bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep,
+                                                                                      m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
+                                                                                      getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                      getSolverInfo().m_jointFeedbackInJointFrame);
+                        }
+                        else
+                        {
+                            btAssert(" RK4Integration is not supported" );
+                        }
+                    }
+                }
+            }
+        }
+    }
+    clearForces();
+    clearMultiBodyForces();
+}
+
+void btDeformableMultiBodyDynamicsWorld::beforeSolverCallbacks(btScalar timeStep)
+{
+    if (0 != m_internalTickCallback)
+    {
+        (*m_internalTickCallback)(this, timeStep);
+    }
+    
+    if (0 != m_solverCallback)
+    {
+        (*m_solverCallback)(m_internalTime, this);
+    }
+}
+
+void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
+{
+    if (0 != m_solverCallback)
+    {
+        (*m_solverCallback)(m_internalTime, this);
+    }
+}
+
+void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force)
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+    bool added = false;
+    for (int i = 0; i < forces.size(); ++i)
+    {
+        if (forces[i]->getForceType() == force->getForceType())
+        {
+            forces[i]->addSoftBody(psb);
+            added = true;
+            break;
+        }
+    }
+    if (!added)
+    {
+        force->addSoftBody(psb);
+        force->setIndices(m_deformableBodySolver->m_objective->getIndices());
+        forces.push_back(force);
+    }
+}
+
+void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
+{
+    m_softBodies.remove(body);
+    btCollisionWorld::removeCollisionObject(body);
+    // force a reinitialize so that node indices get updated.
+    m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1));
+}

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

@@ -1,4 +1,6 @@
 /*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
  Bullet Continuous Collision Detection and Physics Library
  Copyright (c) 2019 Google Inc. http://bulletphysics.org
  This software is provided 'as-is', without any express or implied warranty.
@@ -18,15 +20,20 @@
 #include "btDeformableLagrangianForce.h"
 #include "btDeformableMassSpringForce.h"
 #include "btDeformableBodySolver.h"
+#include "btDeformableMultiBodyConstraintSolver.h"
 #include "btSoftBodyHelpers.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
 #include <functional>
 typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
 
 class btDeformableBodySolver;
 class btDeformableLagrangianForce;
+struct MultiBodyInplaceSolverIslandCallback;
+class btDeformableMultiBodyConstraintSolver;
+
 typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
 
-class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld
+class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
 {
     typedef btAlignedObjectArray<btVector3> TVStack;
 //    using TVStack = btAlignedObjectArray<btVector3>;
@@ -38,10 +45,10 @@ class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld
     bool m_drawFaceTree;
     bool m_drawClusterTree;
     btSoftBodyWorldInfo m_sbi;
-    bool m_ownsSolver;
     btScalar m_internalTime;
+    int m_contact_iterations;
     
-    typedef void (*btSolverCallback)(btScalar time, btDeformableRigidDynamicsWorld* world);
+    typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
     btSolverCallback m_solverCallback;
     
 protected:
@@ -49,13 +56,13 @@ protected:
     
     virtual void integrateTransforms(btScalar timeStep);
     
-    void positionCorrection(btScalar dt);
+    void positionCorrection(btScalar timeStep);
     
-    void solveDeformableBodiesConstraints(btScalar timeStep);
+    void solveConstraints(btScalar timeStep);
     
 public:
-    btDeformableRigidDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0)
+    btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0)
-    : btMultiBodyDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
+    : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
     m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
     {
         m_drawFlags = fDrawFlags::Std;
@@ -65,6 +72,7 @@ public:
         m_sbi.m_broadphase = pairCache;
         m_sbi.m_dispatcher = dispatcher;
         m_sbi.m_sparsesdf.Initialize();
+        m_sbi.m_sparsesdf.setDefaultVoxelsz(0.025);
         m_sbi.m_sparsesdf.Reset();
         
         m_sbi.air_density = (btScalar)1.2;
@@ -72,8 +80,6 @@ public:
         m_sbi.water_offset = 0;
         m_sbi.water_normal = btVector3(0, 0, 0);
         m_sbi.m_gravity.setValue(0, -10, 0);
-        
-        m_sbi.m_sparsesdf.Initialize();
         m_internalTime = 0.0;
     }
 
@@ -82,7 +88,7 @@ public:
         m_solverCallback = cb;
     }
     
-    virtual ~btDeformableRigidDynamicsWorld()
+    virtual ~btDeformableMultiBodyDynamicsWorld()
     {
     }
     
@@ -135,8 +141,18 @@ public:
     
     void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
     
+    void removeSoftBody(btSoftBody* body);
+    
     int getDrawFlags() const { return (m_drawFlags); }
     void setDrawFlags(int f) { m_drawFlags = f; }
+    
+    void setupConstraints();
+    
+    void solveMultiBodyConstraints();
+    
+    void solveMultiBodyRelatedConstraints();
+    
+    void sortConstraints();
 };
 
 #endif  //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H

src/BulletSoftBody/btDeformableNeoHookeanForce.h

@@ -0,0 +1,136 @@
+/*
+Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2019 Google Inc. http://bulletphysics.org
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_NEOHOOKEAN_H
+#define BT_NEOHOOKEAN_H
+
+#include "btDeformableLagrangianForce.h"
+
+class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
+{
+public:
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    btScalar m_mu, m_lambda;
+    btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1)
+    {
+        
+    }
+    
+    btDeformableNeoHookeanForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda)
+    {
+    }
+    
+    virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
+    {
+        addScaledDampingForce(scale, force);
+    }
+    
+    virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+    {
+        addScaledElasticForce(scale, force);
+    }
+    
+    virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+    {
+    }
+    
+    virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+    {
+        int numNodes = getNumNodes();
+        btAssert(numNodes <= force.size())
+        btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_tetras.size(); ++j)
+            {
+                btSoftBody::Tetra& tetra = psb->m_tetras[j];
+                btMatrix3x3 P;
+                firstPiola(tetra.m_F,P);
+                btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+                btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
+                
+                btSoftBody::Node* node0 = tetra.m_n[0];
+                btSoftBody::Node* node1 = tetra.m_n[1];
+                btSoftBody::Node* node2 = tetra.m_n[2];
+                btSoftBody::Node* node3 = tetra.m_n[3];
+                size_t id0 = node0->index;
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                size_t id3 = node3->index;
+                
+                // elastic force
+                // explicit elastic force
+                btScalar scale1 = scale * tetra.m_element_measure;
+                force[id0] -= scale1 * force_on_node0;
+                force[id1] -= scale1 * force_on_node123.getColumn(0);
+                force[id2] -= scale1 * force_on_node123.getColumn(1);
+                force[id3] -= scale1 * force_on_node123.getColumn(2);
+            }
+        }
+    }
+    
+    void firstPiola(const btMatrix3x3& F, btMatrix3x3& P)
+    {
+        btMatrix3x3 C = F.transpose()*F;
+        btScalar J = F.determinant();
+        btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ();
+        P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint() * (m_lambda * (J - 1) - 0.75 * m_mu);
+    }
+    
+    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+    {
+    }
+    
+    void firstPiolaDifferential(const btMatrix3x3& F, const btMatrix3x3& G,  btMatrix3x3& P)
+    {
+        btScalar J = F.determinant();
+        addScaledCofactorMatrixDifferential(F, G, m_lambda*(J-1), P);
+        P += F.adjoint() * m_lambda * DotProduct(F.adjoint(), G);
+    }
+    
+    btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
+    {
+        btScalar ans = 0;
+        for (int i = 0; i < 3; ++i)
+        {
+            for (int j = 0; j < 3; ++j)
+            {
+                ans += A[i][j] * B[i][j];
+            }
+        }
+        return ans;
+    }
+    
+    void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M)
+    {
+        M[0][0] = scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
+        M[1][0] = scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
+        M[2][0] = scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
+        M[0][1] = scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
+        M[1][1] = scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
+        M[2][1] = scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
+        M[0][2] = scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
+        M[1][2] = scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
+        M[2][2] = scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
+    }
+    
+    virtual btDeformableLagrangianForceType getForceType()
+    {
+        return BT_NEOHOOKEAN_FORCE;
+    }
+    
+};
+#endif /* BT_NEOHOOKEAN_H */

src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp

@@ -1,266 +0,0 @@
-/*
- 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.
- */
-
-/* ====== Overview of the Deformable Algorithm ====== */
-
-/*
-A single step of the deformable body simulation contains the following main components:
-1. Update velocity to a temporary state v_{n+1}^* = v_n + explicit_force * dt / mass, where explicit forces include gravity and elastic forces.
-2. Detect collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*.
-3. Then velocities of deformable bodies v_{n+1} are solved in
-        M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass,
-   by a conjugate gradient solver, where the damping force is implicit and depends on v_{n+1}.
-4. Contact constraints are solved as projections as in the paper by Baraff and Witkin https://www.cs.cmu.edu/~baraff/papers/sig98.pdf. Dynamic frictions are treated as a force and added to the rhs of the CG solve, whereas static frictions are treated as constraints similar to contact.
-5. Position is updated via x_{n+1} = x_n + dt * v_{n+1}.
-6. Apply position correction to prevent numerical drift.
-
-The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf
- */
-
-#include <stdio.h>
-#include "btDeformableRigidDynamicsWorld.h"
-#include "btDeformableBodySolver.h"
-#include "LinearMath/btQuickprof.h"
-
-void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
-{
-    BT_PROFILE("internalSingleStepSimulation");
-    reinitialize(timeStep);
-    // add gravity to velocity of rigid and multi bodys
-    applyRigidBodyGravity(timeStep);
-    
-    ///apply gravity and explicit force to velocity, predict motion
-    predictUnconstraintMotion(timeStep);
-    
-    ///perform collision detection
-    btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
-    
-    btMultiBodyDynamicsWorld::calculateSimulationIslands();
-    
-    beforeSolverCallbacks(timeStep);
-    
-    ///solve deformable bodies constraints
-    solveDeformableBodiesConstraints(timeStep);
-    
-    afterSolverCallbacks(timeStep);
-    
-    integrateTransforms(timeStep);
-    
-    ///update vehicle simulation
-    btMultiBodyDynamicsWorld::updateActions(timeStep);
-    
-    btMultiBodyDynamicsWorld::updateActivationState(timeStep);
-    // End solver-wise simulation step
-    // ///////////////////////////////
-}
-
-void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
-{
-    // perform position correction for all constraints 
-    BT_PROFILE("positionCorrection");
-    for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index)
-    {
-        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_deformableBodySolver->m_objective->projection.m_frictions[m_deformableBodySolver->m_objective->projection.m_constraints.getKeyAtIndex(index)];
-        btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index);
-        for (int i = 0; i < constraints.size(); ++i)
-        {
-            DeformableContactConstraint& constraint = constraints[i];
-            DeformableFrictionConstraint& friction = frictions[i];
-            for (int j = 0; j < constraint.m_contact.size(); ++j)
-            {
-                const btSoftBody::RContact* c = constraint.m_contact[j];
-                // skip anchor points
-                if (c == NULL || c->m_node->m_im == 0)
-                    continue;
-                const btSoftBody::sCti& cti = c->m_cti;
-                btVector3 va(0, 0, 0);
-                
-                // grab the velocity of the rigid body
-                if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
-                {
-                    btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
-                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0);
-                }
-                else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
-                {
-                    btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
-                    if (multibodyLinkCol)
-                    {
-                        const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-                        const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
-                        const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
-                        const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
-                        const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
-                        // add in the normal component of the va
-                        btScalar vel = 0.0;
-                        for (int k = 0; k < ndof; ++k)
-                        {
-                            vel += local_v[k] * J_n[k];
-                        }
-                        va = cti.m_normal * vel;
-                        
-                        vel = 0.0;
-                        for (int k = 0; k < ndof; ++k)
-                        {
-                            vel += local_v[k] * J_t1[k];
-                        }
-                        va += c->t1 * vel;
-                        vel = 0.0;
-                        for (int k = 0; k < ndof; ++k)
-                        {
-                            vel += local_v[k] * J_t2[k];
-                        }
-                        va += c->t2 * vel;
-                    }
-                }
-                else
-                {
-                    // The object interacting with deformable node is not supported for position correction
-                    btAssert(false);
-                }
-                
-                if (cti.m_colObj->hasContactResponse())
-                {
-                    btScalar dp = cti.m_offset;
-                    
-                    // only perform position correction when penetrating
-                    if (dp < 0)
-                    {
-                        if (friction.m_static[j] == true)
-                        {
-                            c->m_node->m_v = va;
-                        }
-                        c->m_node->m_v -= dp * cti.m_normal / dt;
-                    }
-                }
-            }
-        }
-    }
-}
-
-
-void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt)
-{
-    BT_PROFILE("integrateTransforms");
-    m_deformableBodySolver->backupVelocity();
-    positionCorrection(dt);
-    btMultiBodyDynamicsWorld::integrateTransforms(dt);
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        for (int j = 0; j < psb->m_nodes.size(); ++j)
-        {
-            btSoftBody::Node& node = psb->m_nodes[j];
-            node.m_x  =  node.m_q + dt * node.m_v;
-        }
-    }
-    m_deformableBodySolver->revertVelocity();
-}
-
-void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep)
-{
-    m_deformableBodySolver->solveConstraints(timeStep);
-}
-
-void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask)
-{
-    m_softBodies.push_back(body);
-    
-    // Set the soft body solver that will deal with this body
-    // to be the world's solver
-    body->setSoftBodySolver(m_deformableBodySolver);
-    
-    btCollisionWorld::addCollisionObject(body,
-                                         collisionFilterGroup,
-                                         collisionFilterMask);
-}
-
-void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
-{
-    BT_PROFILE("predictUnconstraintMotion");
-    btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep);
-    m_deformableBodySolver->predictMotion(timeStep);
-}
-
-void btDeformableRigidDynamicsWorld::reinitialize(btScalar timeStep)
-{
-    m_internalTime += timeStep;
-    m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
-    btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
-    dispatchInfo.m_timeStep = timeStep;
-    dispatchInfo.m_stepCount = 0;
-    dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
-    btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
-}
-
-void btDeformableRigidDynamicsWorld::applyRigidBodyGravity(btScalar timeStep)
-{
-    // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
-    // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
-    // when there are multiple substeps
-    clearForces();
-    clearMultiBodyForces();
-    btMultiBodyDynamicsWorld::applyGravity();
-    // integrate rigid body gravity
-    for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
-    {
-        btRigidBody* rb = m_nonStaticRigidBodies[i];
-        rb->integrateVelocities(timeStep);
-    }
-    // integrate multibody gravity
-    btMultiBodyDynamicsWorld::solveExternalForces(btMultiBodyDynamicsWorld::getSolverInfo());
-    clearForces();
-    clearMultiBodyForces();
-}
-
-void btDeformableRigidDynamicsWorld::beforeSolverCallbacks(btScalar timeStep)
-{
-    if (0 != m_internalTickCallback)
-    {
-        (*m_internalTickCallback)(this, timeStep);
-    }
-    
-    if (0 != m_solverCallback)
-    {
-        (*m_solverCallback)(m_internalTime, this);
-    }
-}
-
-void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
-{
-    if (0 != m_solverCallback)
-    {
-        (*m_solverCallback)(m_internalTime, this);
-    }
-}
-
-void btDeformableRigidDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force)
-{
-    btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
-    bool added = false;
-    for (int i = 0; i < forces.size(); ++i)
-    {
-        if (forces[i]->getForceType() == force->getForceType())
-        {
-            forces[i]->addSoftBody(psb);
-            added = true;
-            break;
-        }
-    }
-    if (!added)
-    {
-        force->addSoftBody(psb);
-        force->setIndices(m_deformableBodySolver->m_objective->getIndices());
-        forces.push_back(force);
-    }
-}

src/BulletSoftBody/btPreconditioner.h

@@ -1,4 +1,6 @@
 /*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
  Bullet Continuous Collision Detection and Physics Library
  Copyright (c) 2019 Google Inc. http://bulletphysics.org
  This software is provided 'as-is', without any express or implied warranty.
@@ -18,9 +20,9 @@ class Preconditioner
 {
 public:
     typedef btAlignedObjectArray<btVector3> TVStack;
-//    using TVStack = btAlignedObjectArray<btVector3>;
     virtual void operator()(const TVStack& x, TVStack& b) = 0;
     virtual void reinitialize(bool nodeUpdated) = 0;
+    virtual ~Preconditioner(){}
 };
 
 class DefaultPreconditioner : public Preconditioner
@@ -34,8 +36,9 @@ public:
     }
     virtual void reinitialize(bool nodeUpdated)
     {
-        
     }
+    
+    virtual ~DefaultPreconditioner(){}
 };
 
 class MassPreconditioner : public Preconditioner

src/BulletSoftBody/btSoftBody.cpp

@@ -20,7 +20,6 @@ subject to the following restrictions:
 #include "LinearMath/btSerializer.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
-
 //
 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
 	: m_softBodySolver(0), m_worldInfo(worldInfo)
@@ -854,6 +853,7 @@ void btSoftBody::scale(const btVector3& scl)
 	updateNormals();
 	updateBounds();
 	updateConstants();
+    initializeDmInverse();
 }
 
 //
@@ -2300,8 +2300,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
-    const btTransform &wtr = (predict) ? tmpCollisionObj->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
+    btTransform wtr = (predict) ?
-
+    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+                 : colObjWrap->getWorldTransform();
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(
 			wtr.invXform(x),
@@ -2811,6 +2812,40 @@ void btSoftBody::setSpringStiffness(btScalar k)
     }
 }
 
+void btSoftBody::initializeDmInverse()
+{
+    btScalar unit_simplex_measure = 1./6.;
+    
+    for (int i = 0; i < m_tetras.size(); ++i)
+    {
+        Tetra &t = m_tetras[i];
+        btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q;
+        btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q;
+        btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q;
+        btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(),
+                       c1.getY(), c2.getY(), c3.getY(),
+                       c1.getZ(), c2.getZ(), c3.getZ());
+        t.m_element_measure = Dm.determinant() * unit_simplex_measure;
+        t.m_Dm_inverse = Dm.inverse();
+    }
+}
+
+void btSoftBody::updateDeformation()
+{
+    for (int i = 0; i < m_tetras.size(); ++i)
+    {
+        // updateDeformation is called before predictMotion where m_q is sync'd.
+        // So m_x is the current position of the node.
+        btSoftBody::Tetra& t = m_tetras[i];
+        btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x;
+        btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x;
+        btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x;
+        btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(),
+                       c1.getY(), c2.getY(), c3.getY(),
+                       c1.getZ(), c2.getZ(), c3.getZ());
+        t.m_F = Ds * t.m_Dm_inverse;
+    }
+}
 //
 void btSoftBody::Joint::Prepare(btScalar dt, int)
 {

src/BulletSoftBody/btSoftBody.h

@@ -292,6 +292,9 @@ public:
 		btVector3 m_c0[4];   // gradients
 		btScalar m_c1;       // (4*kVST)/(im0+im1+im2+im3)
 		btScalar m_c2;       // m_c1/sum(|g0..3|^2)
+        btMatrix3x3 m_Dm_inverse; // rest Dm^-1
+        btMatrix3x3 m_F;
+        btScalar m_element_measure;
 	};
 	/* RContact		*/
 	struct RContact
@@ -1023,6 +1026,8 @@ public:
 	void applyClusters(bool drift);
 	void dampClusters();
     void setSpringStiffness(btScalar k);
+    void initializeDmInverse();
+    void updateDeformation();
 	void applyForces();
 	static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
 	static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);

src/BulletSoftBody/btSoftBodyHelpers.cpp

@@ -16,6 +16,9 @@ subject to the following restrictions:
 
 #include "btSoftBodyInternals.h"
 #include <stdio.h>
+#include <string>
+#include <iostream>
+#include <sstream>
 #include <string.h>
 #include "btSoftBodyHelpers.h"
 #include "LinearMath/btConvexHull.h"
@@ -721,7 +724,8 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const
 										   int resx,
 										   int resy,
 										   int fixeds,
-										   bool gendiags)
+										   bool gendiags,
+                                           btScalar perturbation)
 {
 #define IDX(_x_, _y_) ((_y_)*rx + (_x_))
 	/* Create nodes	*/
@@ -741,7 +745,13 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const
 		for (int ix = 0; ix < rx; ++ix)
 		{
 			const btScalar tx = ix / (btScalar)(rx - 1);
-			x[IDX(ix, iy)] = lerp(py0, py1, tx);
+            btScalar pert = perturbation * btScalar(rand())/RAND_MAX;
+            btVector3 temp1 = py1;
+            temp1.setY(py1.getY() + pert);
+            btVector3 temp = py0;
+            pert = perturbation * btScalar(rand())/RAND_MAX;
+            temp.setY(py0.getY() + pert);
+			x[IDX(ix, iy)] = lerp(temp, temp1, tx);
 			m[IDX(ix, iy)] = 1;
 		}
 	}
@@ -1221,9 +1231,102 @@ if(face&&face[0])
 			}
 		}
 	}
+    psb->initializeDmInverse();
 	printf("Nodes:  %u\r\n", psb->m_nodes.size());
 	printf("Links:  %u\r\n", psb->m_links.size());
 	printf("Faces:  %u\r\n", psb->m_faces.size());
 	printf("Tetras: %u\r\n", psb->m_tetras.size());
 	return (psb);
 }
+
+btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file)
+{
+    std::ifstream fs;
+    fs.open(vtk_file);
+    btAssert(fs);
+    
+    typedef btAlignedObjectArray<int> Index;
+    std::string line;
+    btAlignedObjectArray<btVector3> X;
+    btVector3 position;
+    btAlignedObjectArray<Index> indices;
+    bool reading_points = false;
+    bool reading_tets = false;
+    size_t n_points = 0;
+    size_t n_tets = 0;
+    size_t x_count = 0;
+    size_t indices_count = 0;
+    while (std::getline(fs, line))
+    {
+        std::stringstream ss(line);
+        if (line.size() == (size_t)(0))
+        {
+        }
+        else if (line.substr(0, 6) == "POINTS")
+        {
+            reading_points = true;
+            reading_tets = false;
+            ss.ignore(128, ' '); // ignore "POINTS"
+            ss >> n_points;
+            X.resize(n_points);
+        }
+        else if (line.substr(0, 5) == "CELLS")
+        {
+            reading_points = false;
+            reading_tets = true;
+            ss.ignore(128, ' '); // ignore "CELLS"
+            ss >> n_tets;
+            indices.resize(n_tets);
+        }
+        else if (line.substr(0, 10) == "CELL_TYPES")
+        {
+            reading_points = false;
+            reading_tets = false;
+        }
+        else if (reading_points)
+        {
+            btScalar p;
+            ss >> p;
+            position.setX(p);
+            ss >> p;
+            position.setY(p);
+            ss >> p;
+            position.setZ(p);
+            X[x_count++] = position;
+        }
+        else if (reading_tets)
+        {
+            ss.ignore(128, ' '); // ignore "4"
+            Index tet;
+            tet.resize(4);
+            for (size_t i = 0; i < 4; i++)
+            {
+                ss >> tet[i];
+            }
+            indices[indices_count++] = tet;
+        }
+    }
+    btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0);
+    
+    for (int i = 0; i < n_tets; ++i)
+    {
+        const Index& ni = indices[i];
+        psb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
+        {
+            psb->appendLink(ni[0], ni[1], 0, true);
+            psb->appendLink(ni[1], ni[2], 0, true);
+            psb->appendLink(ni[2], ni[0], 0, true);
+            psb->appendLink(ni[0], ni[3], 0, true);
+            psb->appendLink(ni[1], ni[3], 0, true);
+            psb->appendLink(ni[2], ni[3], 0, true);
+        }
+    }
+    psb->initializeDmInverse();
+    printf("Nodes:  %u\r\n", psb->m_nodes.size());
+    printf("Links:  %u\r\n", psb->m_links.size());
+    printf("Faces:  %u\r\n", psb->m_faces.size());
+    printf("Tetras: %u\r\n", psb->m_tetras.size());
+
+    fs.close();
+    return psb;
+}

src/BulletSoftBody/btSoftBodyHelpers.h

@@ -17,7 +17,8 @@ subject to the following restrictions:
 #define BT_SOFT_BODY_HELPERS_H
 
 #include "btSoftBody.h"
-
+#include <fstream>
+#include <string>
 //
 // Helpers
 //
@@ -91,7 +92,8 @@ struct btSoftBodyHelpers
 								   int resx,
 								   int resy,
 								   int fixeds,
-								   bool gendiags);
+								   bool gendiags,
+                                   btScalar perturbation = 0.);
 	/* Create a patch with UV Texture Coordinates	*/
 	static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
 									 const btVector3& corner00,
@@ -140,6 +142,9 @@ struct btSoftBodyHelpers
 											bool bfacelinks,
 											bool btetralinks,
 											bool bfacesfromtetras);
+    static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
+
+    
 
 	/// Sort the list of links to move link calculations that are dependent upon earlier
 	/// ones as far as possible away from the calculation of those values

src/BulletSoftBody/btSoftBodyInternals.h

@@ -998,7 +998,7 @@ struct btSoftColliders
 			if (!n.m_battach)
             {
                 // check for collision at x_{n+1}^*
-                if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true))
+                if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true))
                 {
                     const btScalar ima = n.m_im;
                     const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
@@ -1006,7 +1006,7 @@ struct btSoftColliders
                     if (ms > 0)
                     {
                         // resolve contact at x_n
-                        psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false);
+                        psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ false);
                         btSoftBody::sCti& cti = c.m_cti;
                         c.m_node = &n;
                         const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();

src/BulletSoftBody/btSoftBodySolvers.h

@@ -72,10 +72,10 @@ public:
 	virtual void copyBackToSoftBodies(bool bMove = true) = 0;
 
 	/** Predict motion of soft bodies into next timestep */
-	virtual void predictMotion(float solverdt) = 0;
+	virtual void predictMotion(btScalar solverdt) = 0;
 
 	/** Solve constraints for a set of soft bodies */
-	virtual void solveConstraints(float solverdt) = 0;
+	virtual void solveConstraints(btScalar solverdt) = 0;
 
 	/** Perform necessary per-step updates of soft bodies such as recomputing normals and bounding boxes */
 	virtual void updateSoftBodies() = 0;

src/BulletSoftBody/btSparseSDF.h

@@ -70,6 +70,7 @@ struct btSparseSdf
 
 	btAlignedObjectArray<Cell*> cells;
 	btScalar voxelsz;
+    btScalar m_defaultVoxelsz;
 	int puid;
 	int ncells;
 	int m_clampCells;
@@ -87,9 +88,16 @@ struct btSparseSdf
 		//if this limit is reached, the SDF is reset (at the cost of some performance during the reset)
 		m_clampCells = clampCells;
 		cells.resize(hashsize, 0);
+        m_defaultVoxelsz = 0.25;
 		Reset();
 	}
 	//
+    
+    void setDefaultVoxelsz(btScalar sz)
+    {
+        m_defaultVoxelsz = sz;
+    }
+    
 	void Reset()
 	{
 		for (int i = 0, ni = cells.size(); i < ni; ++i)
@@ -103,7 +111,7 @@ struct btSparseSdf
 				pc = pn;
 			}
 		}
-		voxelsz = 0.25;
+		voxelsz = m_defaultVoxelsz;
 		puid = 0;
 		ncells = 0;
 		nprobes = 1;