Merge pull request #2351 from xhan0619/Deformable

Add deformable body world and solver

examples/CMakeLists.txt

@@ -1,4 +1,4 @@
-SUBDIRS( HelloWorld BasicDemo )
+SUBDIRS( HelloWorld BasicDemo)
 IF(BUILD_BULLET3)
 	SUBDIRS( ExampleBrowser RobotSimulator SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow TwoJoint )
 ENDIF()

examples/DeformableDemo/DeformableMultibody.cpp

@@ -0,0 +1,409 @@
+/*
+ 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.
+ */
+
+///create 125 (5x5x5) dynamic object
+#define ARRAY_SIZE_X 5
+#define ARRAY_SIZE_Y 5
+#define ARRAY_SIZE_Z 5
+
+//maximum number of objects (and allow user to shoot additional boxes)
+#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
+
+///scaling of the objects (0.1 = 20 centimeter boxes )
+#define SCALING 1.
+#define START_POS_X -5
+#define START_POS_Y -5
+#define START_POS_Z -3
+
+#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/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+#include "../SoftDemo/BunnyMesh.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
+
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+///The DeformableMultibody demo deformable bodies self-collision
+static bool g_floatingBase = true;
+static float friction = 1.;
+class DeformableMultibody : public CommonMultiBodyBase
+{
+    btMultiBody* m_multiBody;
+    btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
+public:
+	DeformableMultibody(struct GUIHelperInterface* helper)
+		: CommonMultiBodyBase(helper)
+	{
+	}
+    
+	virtual ~DeformableMultibody()
+	{
+	}
+    
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 30;
+        float pitch = -30;
+        float yaw = 100;
+        float targetPos[3] = {0, -10, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+    
+    virtual void stepSimulation(float deltaTime);
+    
+    btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false);
+    
+    void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
+    
+
+    virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
+    {
+        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld()
+    {
+        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual void renderScene()
+    {
+        CommonMultiBodyBase::renderScene();
+        btDeformableRigidDynamicsWorld* 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 DeformableMultibody::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+	///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+	m_broadphase = new btDbvtBroadphase();
+    btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
+    btMultiBodyConstraintSolver* sol;
+    sol = new btMultiBodyConstraintSolver;
+	m_solver = sol;
+    
+    m_dynamicsWorld = new btDeformableRigidDynamicsWorld(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;
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    {
+        ///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, -40, 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.5);
+
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body,1,1+2);
+    }
+
+    
+    {
+        bool damping = true;
+        bool gyro = false;
+        int numLinks = 4;
+        bool spherical = false;  //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
+        bool canSleep = false;
+        bool selfCollide = true;
+        btVector3 linkHalfExtents(.4, 1, .4);
+        btVector3 baseHalfExtents(.4, 1, .4);
+        
+        btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
+        
+        mbC->setCanSleep(canSleep);
+        mbC->setHasSelfCollision(selfCollide);
+        mbC->setUseGyroTerm(gyro);
+        //
+        if (!damping)
+        {
+            mbC->setLinearDamping(0.0f);
+            mbC->setAngularDamping(0.0f);
+        }
+        else
+        {
+            mbC->setLinearDamping(0.04f);
+            mbC->setAngularDamping(0.04f);
+        }
+
+        if (numLinks > 0)
+        {
+            btScalar q0 = 0.f * SIMD_PI / 180.f;
+            if (!spherical)
+            {
+                mbC->setJointPosMultiDof(0, &q0);
+            }
+            else
+            {
+                btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
+                quat0.normalize();
+                mbC->setJointPosMultiDof(0, quat0);
+            }
+        }
+        ///
+        addColliders_testMultiDof(mbC, m_dynamicsWorld, baseHalfExtents, linkHalfExtents);
+    }
+    
+    // create a patch of cloth
+    {
+        btScalar h = 0;
+        const btScalar s = 4;
+        btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
+                                                         btVector3(+s, h, -s),
+                                                         btVector3(-s, h, +s),
+                                                         btVector3(+s, h, +s),
+                                                         20,20,
+//                                                         3,3,
+                                                         1 + 2 + 4 + 8, true);
+
+        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;
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
+    }
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void DeformableMultibody::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 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;
+}
+
+void DeformableMultibody::stepSimulation(float deltaTime)
+{
+//    getDeformableDynamicsWorld()->getMultiBodyDynamicsWorld()->stepSimulation(deltaTime);
+    m_dynamicsWorld->stepSimulation(deltaTime, 5, 1./250.);
+}
+
+
+btMultiBody* DeformableMultibody::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating)
+{
+    //init the base
+    btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
+    float baseMass = .1f;
+    
+    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);
+    btVector3 vel(0, 0, 0);
+    //    pMultiBody->setBaseVel(vel);
+    
+    //init the links
+    btVector3 hingeJointAxis(1, 0, 0);
+    float linkMass = .1f;
+    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
+    btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0);                      //par body's COM to cur body's COM offset
+    btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0);                         //cur body's COM to cur body's PIV offset
+    btVector3 parentComToCurrentPivot = parentComToCurrentCom - 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)
+    {
+        if (!spherical)
+            pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
+        else
+            //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
+            pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
+    }
+    
+    pMultiBody->finalizeMultiDof();
+    
+    ///
+    pWorld->addMultiBody(pMultiBody);
+    ///
+    return pMultiBody;
+}
+
+void DeformableMultibody::addColliders_testMultiDof(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();
+    
+    {
+        //    float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
+        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];
+        //    float pos[4]={posr.x(),posr.y(),posr.z(),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* DeformableMultibodyCreateFunc(struct CommonExampleOptions& options)
+{
+	return new DeformableMultibody(options.m_guiHelper);
+}
+
+

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

examples/DeformableDemo/DeformableRigid.cpp

@@ -0,0 +1,291 @@
+/*
+ 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.
+ */
+
+///create 125 (5x5x5) dynamic object
+#define ARRAY_SIZE_X 5
+#define ARRAY_SIZE_Y 5
+#define ARRAY_SIZE_Z 5
+
+//maximum number of objects (and allow user to shoot additional boxes)
+#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
+
+///scaling of the objects (0.1 = 20 centimeter boxes )
+#define SCALING 1.
+#define START_POS_X -5
+#define START_POS_Y -5
+#define START_POS_Z -3
+
+#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/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The DeformableRigid 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).
+class DeformableRigid : public CommonRigidBodyBase
+{
+public:
+	DeformableRigid(struct GUIHelperInterface* helper)
+		: CommonRigidBodyBase(helper)
+	{
+	}
+	virtual ~DeformableRigid()
+	{
+	}
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 20;
+        float pitch = -45;
+        float yaw = 100;
+        float targetPos[3] = {0, -3, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+    
+    void stepSimulation(float deltaTime)
+    {
+        //use a smaller internal timestep, there are stability issues
+        float internalTimeStep = 1. / 240.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+    }
+    
+    void Ctor_RbUpStack(int count)
+    {
+        float mass = 0.2;
+        
+        btCompoundShape* cylinderCompound = new btCompoundShape;
+        btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5));
+        btCollisionShape* boxShape = new btBoxShape(btVector3(2, .5, .5));
+        btTransform localTransform;
+        localTransform.setIdentity();
+        cylinderCompound->addChildShape(localTransform, boxShape);
+        btQuaternion orn(SIMD_HALF_PI, 0, 0);
+        localTransform.setRotation(orn);
+        //    localTransform.setOrigin(btVector3(1,1,1));
+        cylinderCompound->addChildShape(localTransform, cylinderShape);
+        
+        btCollisionShape* shape[] = {
+            new btBoxShape(btVector3(1, 1, 1)),
+//            new btSphereShape(0.75),
+//            cylinderCompound
+        };
+//        static const int nshapes = sizeof(shape) / sizeof(shape[0]);
+//        for (int i = 0; i < count; ++i)
+//        {
+//            btTransform startTransform;
+//            startTransform.setIdentity();
+//            startTransform.setOrigin(btVector3(0, 2+ 2 * i, 0));
+//            startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
+//            createRigidBody(mass, startTransform, shape[i % nshapes]);
+//        }
+        btTransform startTransform;
+        startTransform.setIdentity();
+        startTransform.setOrigin(btVector3(1, 1.5, 1));
+        createRigidBody(mass, startTransform, shape[0]);
+        startTransform.setOrigin(btVector3(1, 1.5, -1));
+        createRigidBody(mass, startTransform, shape[0]);
+        startTransform.setOrigin(btVector3(-1, 1.5, 1));
+        createRigidBody(mass, startTransform, shape[0]);
+        startTransform.setOrigin(btVector3(-1, 1.5, -1));
+        createRigidBody(mass, startTransform, shape[0]);
+        startTransform.setOrigin(btVector3(0, 3.5, 0));
+        createRigidBody(mass, startTransform, shape[0]);
+    }
+    
+    virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
+    {
+        ///just make it a btSoftRigidDynamicsWorld please
+        ///or we will add type checking
+        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld()
+    {
+        ///just make it a btSoftRigidDynamicsWorld please
+        ///or we will add type checking
+        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual void renderScene()
+    {
+        CommonRigidBodyBase::renderScene();
+        btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
+            //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
+            {
+                btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+            }
+        }
+    }
+};
+
+void DeformableRigid::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+	///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+	m_broadphase = new btDbvtBroadphase();
+    btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
+
+	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
+	btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver();
+	m_solver = sol;
+
+	m_dynamicsWorld = new btDeformableRigidDynamicsWorld(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;
+    
+//    getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    {
+        ///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, -32, 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(1);
+
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body);
+    }
+    
+    // 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),
+//                                                         3,3,
+                                                          20,20,
+                                                          1 + 2 + 4 + 8, true);
+//                                                          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(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));
+        
+        // add a few rigid bodies
+        Ctor_RbUpStack(1);
+    }
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void DeformableRigid::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 collision shapes
+	for (int j = 0; j < m_collisionShapes.size(); j++)
+	{
+		btCollisionShape* shape = m_collisionShapes[j];
+		delete shape;
+	}
+	m_collisionShapes.clear();
+
+	delete m_dynamicsWorld;
+
+	delete m_solver;
+
+	delete m_broadphase;
+
+	delete m_dispatcher;
+
+	delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* DeformableRigidCreateFunc(struct CommonExampleOptions& options)
+{
+	return new DeformableRigid(options.m_guiHelper);
+}
+
+

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

examples/DeformableDemo/Pinch.cpp

@@ -0,0 +1,397 @@
+/*
+ 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.
+ */
+
+///create 125 (5x5x5) dynamic object
+#define ARRAY_SIZE_X 5
+#define ARRAY_SIZE_Y 5
+#define ARRAY_SIZE_Z 5
+
+//maximum number of objects (and allow user to shoot additional boxes)
+#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
+
+///scaling of the objects (0.1 = 20 centimeter boxes )
+#define SCALING 1.
+#define START_POS_X -5
+#define START_POS_Y -5
+#define START_POS_Z -3
+
+#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/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The Pinch 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).
+
+struct TetraCube
+{
+#include "../SoftDemo/cube.inl"
+};
+
+struct TetraBunny
+{
+#include "../SoftDemo/bunny.inl"
+};
+
+
+class Pinch : public CommonRigidBodyBase
+{
+public:
+	Pinch(struct GUIHelperInterface* helper)
+		: CommonRigidBodyBase(helper)
+	{
+	}
+	virtual ~Pinch()
+	{
+	}
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 25;
+        float pitch = -30;
+        float yaw = 100;
+        float targetPos[3] = {0, -0, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+    
+    void stepSimulation(float deltaTime)
+    {
+        //use a smaller internal timestep, there are stability issues
+        float internalTimeStep = 1. / 240.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+    }
+    
+    void createGrip()
+    {
+        int count = 2;
+        float mass = 1e6;
+        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 btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
+    {
+        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld()
+    {
+        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual void renderScene()
+    {
+        CommonRigidBodyBase::renderScene();
+        btDeformableRigidDynamicsWorld* 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 dynamics(btScalar time, btDeformableRigidDynamicsWorld* world)
+{
+    btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
+    if (rbs.size()<2)
+        return;
+    btRigidBody* rb0 = rbs[0];
+    btScalar pressTime = 0.9;
+    btScalar liftTime = 2.5;
+    btScalar shiftTime = 3.5;
+    btScalar holdTime = 4.5*1000;
+    btScalar dropTime = 5.3*1000;
+    btTransform rbTransform;
+    rbTransform.setIdentity();
+    btVector3 translation;
+    btVector3 velocity;
+    
+    btVector3 initialTranslationLeft = btVector3(0.5,3,4);
+    btVector3 initialTranslationRight = btVector3(0.5,3,-4);
+    btVector3 pinchVelocityLeft = btVector3(0,0,-2);
+    btVector3 pinchVelocityRight = btVector3(0,0,2);
+    btVector3 liftVelocity = btVector3(0,5,0);
+    btVector3 shiftVelocity = btVector3(0,0,5);
+    btVector3 holdVelocity = btVector3(0,0,0);
+    btVector3 openVelocityLeft = btVector3(0,0,4);
+    btVector3 openVelocityRight = btVector3(0,0,-4);
+    
+    if (time < pressTime)
+    {
+        velocity = pinchVelocityLeft;
+        translation = initialTranslationLeft + pinchVelocityLeft * time;
+    }
+    else if (time < liftTime)
+    {
+        velocity = liftVelocity;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime);
+        
+    }
+    else if (time < shiftTime)
+    {
+        velocity = shiftVelocity;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
+    }
+    else if (time < holdTime)
+    {
+        velocity = btVector3(0,0,0);
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
+    }
+    else if (time < dropTime)
+    {
+        velocity = openVelocityLeft;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime);
+    }
+    else
+    {
+        velocity = holdVelocity;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime);
+    }
+    rbTransform.setOrigin(translation);
+    rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
+    rb0->setCenterOfMassTransform(rbTransform);
+    rb0->setAngularVelocity(btVector3(0,0,0));
+    rb0->setLinearVelocity(velocity);
+    
+    btRigidBody* rb1 = rbs[1];
+    if (time < pressTime)
+    {
+        velocity = pinchVelocityRight;
+        translation = initialTranslationRight + pinchVelocityRight * time;
+    }
+    else if (time < liftTime)
+    {
+        velocity = liftVelocity;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime);
+        
+    }
+    else if (time < shiftTime)
+    {
+        velocity = shiftVelocity;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
+    }
+    else if (time < holdTime)
+    {
+        velocity = btVector3(0,0,0);
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
+    }
+    else if (time < dropTime)
+    {
+        velocity = openVelocityRight;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime);
+    }
+    else
+    {
+        velocity = holdVelocity;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime);
+    }
+    rbTransform.setOrigin(translation);
+    rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
+    rb1->setCenterOfMassTransform(rbTransform);
+    rb1->setAngularVelocity(btVector3(0,0,0));
+    rb1->setLinearVelocity(velocity);
+    
+    rb0->setFriction(20);
+    rb1->setFriction(20);
+}
+
+void Pinch::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+	m_broadphase = new btDbvtBroadphase();
+    btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
+
+	btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver();
+	m_solver = sol;
+
+	m_dynamicsWorld = new btDeformableRigidDynamicsWorld(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;
+    
+    getDeformableDynamicsWorld()->setSolverCallback(dynamics);
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    //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, 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.5);
+
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body);
+    }
+    
+    // create a soft block
+    {
+        btScalar verts[24] = {0.f, 0.f, 0.f,
+            1.f, 0.f, 0.f,
+            0.f, 1.f, 0.f,
+            0.f, 0.f, 1.f,
+            1.f, 1.f, 0.f,
+            0.f, 1.f, 1.f,
+            1.f, 0.f, 1.f,
+            1.f, 1.f, 1.f
+        };
+        int triangles[60] = {0, 6, 3,
+            0,1,6,
+            7,5,3,
+            7,3,6,
+            4,7,6,
+            4,6,1,
+            7,2,5,
+            7,4,2,
+            0,3,2,
+            2,3,5,
+            0,2,4,
+            0,4,1,
+            0,6,5,
+            0,6,4,
+            3,4,2,
+            3,4,7,
+            2,7,3,
+            2,7,1,
+            4,5,0,
+            4,5,6,
+        };
+//       btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(getDeformableDynamicsWorld()->getWorldInfo(), &verts[0], &triangles[0], 20);
+////
+        btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
+                                                                  TetraCube::getElements(),
+                                                                  0,
+                                                                  TetraCube::getNodes(),
+                                                                  false, true, true);
+        
+        psb->scale(btVector3(2, 2, 2));
+        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));
+        // add a grippers
+        createGrip();
+    }
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void Pinch::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 collision shapes
+	for (int j = 0; j < m_collisionShapes.size(); j++)
+	{
+		btCollisionShape* shape = m_collisionShapes[j];
+		delete shape;
+	}
+	m_collisionShapes.clear();
+
+	delete m_dynamicsWorld;
+
+	delete m_solver;
+
+	delete m_broadphase;
+
+	delete m_dispatcher;
+
+	delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* PinchCreateFunc(struct CommonExampleOptions& options)
+{
+	return new Pinch(options.m_guiHelper);
+}
+
+

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

examples/DeformableDemo/VolumetricDeformable.cpp

@@ -0,0 +1,296 @@
+/*
+ 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.
+ */
+
+///create 125 (5x5x5) dynamic object
+#define ARRAY_SIZE_X 5
+#define ARRAY_SIZE_Y 5
+#define ARRAY_SIZE_Z 5
+
+//maximum number of objects (and allow user to shoot additional boxes)
+#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
+
+///scaling of the objects (0.1 = 20 centimeter boxes )
+#define SCALING 1.
+#define START_POS_X -5
+#define START_POS_Y -5
+#define START_POS_Z -3
+
+#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/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The VolumetricDeformable 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).
+
+
+struct TetraCube
+{
+#include "../SoftDemo/cube.inl"
+};
+
+class VolumetricDeformable : public CommonRigidBodyBase
+{
+public:
+	VolumetricDeformable(struct GUIHelperInterface* helper)
+		: CommonRigidBodyBase(helper)
+	{
+	}
+	virtual ~VolumetricDeformable()
+	{
+	}
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 20;
+        float pitch = -45;
+        float yaw = 100;
+        float targetPos[3] = {0, 3, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+    
+    void stepSimulation(float deltaTime)
+    {
+        //use a smaller internal timestep, there are stability issues
+        float internalTimeStep = 1. / 240.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+    }
+    
+    void createStaticBox(const btVector3& halfEdge, const btVector3& translation)
+    {
+        btCollisionShape* box = new btBoxShape(halfEdge);
+        m_collisionShapes.push_back(box);
+        
+        btTransform Transform;
+        Transform.setIdentity();
+        Transform.setOrigin(translation);
+        Transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.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)
+            box->calculateLocalInertia(mass, localInertia);
+        //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+        btDefaultMotionState* myMotionState = new btDefaultMotionState(Transform);
+        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, box, localInertia);
+        btRigidBody* body = new btRigidBody(rbInfo);
+        body->setFriction(0.5);
+        
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body);
+    }
+    
+    void Ctor_RbUpStack(int count)
+    {
+        float mass = 0.02;
+        
+        btCompoundShape* cylinderCompound = new btCompoundShape;
+        btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5));
+        btCollisionShape* boxShape = new btBoxShape(btVector3(2, .5, .5));
+        btTransform localTransform;
+        localTransform.setIdentity();
+        cylinderCompound->addChildShape(localTransform, boxShape);
+        btQuaternion orn(SIMD_HALF_PI, 0, 0);
+        localTransform.setRotation(orn);
+        //    localTransform.setOrigin(btVector3(1,1,1));
+        cylinderCompound->addChildShape(localTransform, cylinderShape);
+        
+        btCollisionShape* shape[] = {
+            new btBoxShape(btVector3(1, 1, 1)),
+        };
+        static const int nshapes = sizeof(shape) / sizeof(shape[0]);
+        for (int i = 0; i < count; ++i)
+        {
+            btTransform startTransform;
+            startTransform.setIdentity();
+            startTransform.setOrigin(btVector3(i, 10 + 2 * i, i-1));
+            createRigidBody(mass, startTransform, shape[i % nshapes]);
+        }
+    }
+    
+    virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
+    {
+        ///just make it a btSoftRigidDynamicsWorld please
+        ///or we will add type checking
+        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld()
+    {
+        ///just make it a btSoftRigidDynamicsWorld please
+        ///or we will add type checking
+        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual void renderScene()
+    {
+        CommonRigidBodyBase::renderScene();
+        btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
+            //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
+            {
+                btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+            }
+        }
+    }
+};
+
+void VolumetricDeformable::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+	///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+	m_broadphase = new btDbvtBroadphase();
+    btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
+
+	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
+	btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver();
+	m_solver = sol;
+
+	m_dynamicsWorld = new btDeformableRigidDynamicsWorld(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;
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    {
+        ///create a ground
+        btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(50.), btScalar(150.)));
+        m_collisionShapes.push_back(groundShape);
+
+        btTransform groundTransform;
+        groundTransform.setIdentity();
+        groundTransform.setOrigin(btVector3(0, -50, 0));
+        groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.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.5);
+
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body);
+    }
+    
+    createStaticBox(btVector3(1, 5, 5), btVector3(-5,0,0));
+    createStaticBox(btVector3(1, 5, 5), btVector3(5,0,0));
+    createStaticBox(btVector3(5, 5, 1), btVector3(0,0,5));
+    createStaticBox(btVector3(5, 5, 1), btVector3(0,0,-5));
+    
+    // create volumetric soft body
+    {
+        btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
+                                                                  TetraCube::getElements(),
+                                                                  0,
+                                                                  TetraCube::getNodes(),
+                                                                  false, true, true);
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+        psb->scale(btVector3(2, 2, 2));
+        psb->translate(btVector3(0, 5, 0));
+//        psb->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));
+    }
+    // add a few rigid bodies
+    Ctor_RbUpStack(4); 
+    
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void VolumetricDeformable::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 collision shapes
+	for (int j = 0; j < m_collisionShapes.size(); j++)
+	{
+		btCollisionShape* shape = m_collisionShapes[j];
+		delete shape;
+	}
+	m_collisionShapes.clear();
+
+	delete m_dynamicsWorld;
+
+	delete m_solver;
+
+	delete m_broadphase;
+
+	delete m_dispatcher;
+
+	delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* VolumetricDeformableCreateFunc(struct CommonExampleOptions& options)
+{
+	return new VolumetricDeformable(options.m_guiHelper);
+}
+
+

examples/DeformableDemo/VolumetricDeformable.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 _VOLUMETRIC_DEFORMABLE_H
+#define _VOLUMETRIC_DEFORMABLE_H
+
+class CommonExampleInterface* VolumetricDeformableCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_VOLUMETRIC_DEFORMABLE__H

examples/ExampleBrowser/CMakeLists.txt

@@ -359,6 +359,14 @@ SET(BulletExampleBrowser_SRCS
  ../MultiBody/MultiBodyConstraintFeedback.cpp
   ../SoftDemo/SoftDemo.cpp
   ../SoftDemo/SoftDemo.h
+  ../DeformableDemo/Pinch.cpp
+  ../DeformableDemo/Pinch.h
+  ../DeformableDemo/DeformableMultibody.cpp
+  ../DeformableDemo/DeformableMultibody.h
+  ../DeformableDemo/DeformableRigid.cpp
+  ../DeformableDemo/DeformableRigid.h
+  ../DeformableDemo/VolumetricDeformable.cpp
+  ../DeformableDemo/VolumetricDeformable.h
   ../MultiBody/MultiDofDemo.cpp
   ../MultiBody/MultiDofDemo.h
   ../RigidBody/RigidBodySoftContact.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -47,6 +47,10 @@
 #include "../FractureDemo/FractureDemo.h"
 #include "../DynamicControlDemo/MotorDemo.h"
 #include "../RollingFrictionDemo/RollingFrictionDemo.h"
+#include "../DeformableDemo/DeformableRigid.h"
+#include "../DeformableDemo/Pinch.h"
+#include "../DeformableDemo/DeformableMultibody.h"
+#include "../DeformableDemo/VolumetricDeformable.h"
 #include "../SharedMemory/PhysicsServerExampleBullet2.h"
 #include "../SharedMemory/PhysicsServerExample.h"
 #include "../SharedMemory/PhysicsClientExample.h"
@@ -117,7 +121,7 @@ static ExampleEntry gDefaultExamples[] =
 		ExampleEntry(1, "Basic Example", "Create some rigid bodies using box collision shapes. This is a good example to familiarize with the basic initialization of Bullet. The Basic Example can also be compiled without graphical user interface, as a console application. Press W for wireframe, A to show AABBs, I to suspend/restart physics simulation. Press D to toggle auto-deactivation of the simulation. ", BasicExampleCreateFunc),
 
 		ExampleEntry(1, "Rolling Friction", "Damping is often not good enough to keep rounded objects from rolling down a sloped surface. Instead, you can set the rolling friction of a rigid body. Generally it is best to leave the rolling friction to zero, to avoid artifacts.", RollingFrictionCreateFunc),
-
+        
 		ExampleEntry(1, "Constraints", "Show the use of the various constraints in Bullet. Press the L key to visualize the constraint limits. Press the C key to visualize the constraint frames.",
 					 AllConstraintCreateFunc),
 
@@ -190,6 +194,13 @@ static ExampleEntry gDefaultExamples[] =
 		ExampleEntry(1, "Spheres & Plane C-API (Bullet2)", "Collision C-API using Bullet 2.x backend", CollisionTutorialBullet2CreateFunc, TUT_SPHERE_PLANE_BULLET2),
 //ExampleEntry(1, "Spheres & Plane C-API (Bullet3)", "Collision C-API using Bullet 3.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_RTB3),
 
+        ExampleEntry(0, "Deformabe Body"),
+        ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableRigidCreateFunc),
+        ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc),
+        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),
+        
 #ifdef INCLUDE_CLOTH_DEMOS
 		ExampleEntry(0, "Soft Body"),
 		ExampleEntry(1, "Cloth", "Simulate a patch of cloth.", SoftDemoCreateFunc, 0),

examples/ExampleBrowser/premake4.lua

@@ -182,6 +182,7 @@ project "App_BulletExampleBrowser"
 		"../RenderingExamples/*",
 		"../VoronoiFracture/*",
 		"../SoftDemo/*",
+		"../DeformableDemo/*",
 		"../RollingFrictionDemo/*",
 		"../rbdl/*",
 		"../FractureDemo/*",

examples/MultiBodyBaseline/MultiBodyBaseline.cpp

@@ -0,0 +1,358 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+///create 125 (5x5x5) dynamic object
+#define ARRAY_SIZE_X 5
+#define ARRAY_SIZE_Y 5
+#define ARRAY_SIZE_Z 5
+
+//maximum number of objects (and allow user to shoot additional boxes)
+#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
+
+///scaling of the objects (0.1 = 20 centimeter boxes )
+#define SCALING 1.
+#define START_POS_X -5
+#define START_POS_Y -5
+#define START_POS_Z -3
+
+#include "MultiBodyBaseline.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h"
+#include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+#include "../SoftDemo/BunnyMesh.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
+
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+///The MultiBodyBaseline demo deformable bodies self-collision
+static bool g_floatingBase = true;
+static float friction = 1.;
+class MultiBodyBaseline : public CommonMultiBodyBase
+{
+    btMultiBody* m_multiBody;
+    btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
+public:
+	MultiBodyBaseline(struct GUIHelperInterface* helper)
+		: CommonMultiBodyBase(helper)
+	{
+	}
+    
+	virtual ~MultiBodyBaseline()
+	{
+	}
+    
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 30;
+        float pitch = -30;
+        float yaw = 100;
+        float targetPos[3] = {0, -10, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+    
+    virtual void stepSimulation(float deltaTime);
+    
+    btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false);
+    
+    void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
+
+};
+
+void MultiBodyBaseline::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+	///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+	m_broadphase = new btDbvtBroadphase();
+    btMultiBodyConstraintSolver* sol;
+    sol = new btMultiBodyConstraintSolver;
+	m_solver = sol;
+    
+    btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration);
+    m_dynamicsWorld = world;
+    //    m_dynamicsWorld->setDebugDrawer(&gDebugDraw);
+    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+    m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
+
+    {
+        ///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, -40, 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.5);
+
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body,1,1+2);
+    }
+
+    
+    {
+        bool damping = true;
+        bool gyro = false;
+        int numLinks = 4;
+        bool spherical = false;  //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
+        bool canSleep = false;
+        bool selfCollide = true;
+        btVector3 linkHalfExtents(1, 1, 1);
+        btVector3 baseHalfExtents(1, 1, 1);
+        
+        btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
+        
+        mbC->setCanSleep(canSleep);
+        mbC->setHasSelfCollision(selfCollide);
+        mbC->setUseGyroTerm(gyro);
+        //
+        if (!damping)
+        {
+            mbC->setLinearDamping(0.0f);
+            mbC->setAngularDamping(0.0f);
+        }
+        else
+        {
+            mbC->setLinearDamping(0.04f);
+            mbC->setAngularDamping(0.04f);
+        }
+
+        if (numLinks > 0)
+        {
+            btScalar q0 = 0.f * SIMD_PI / 180.f;
+            if (!spherical)
+            {
+                mbC->setJointPosMultiDof(0, &q0);
+            }
+            else
+            {
+                btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
+                quat0.normalize();
+                mbC->setJointPosMultiDof(0, quat0);
+            }
+        }
+        ///
+        addColliders_testMultiDof(mbC, m_dynamicsWorld, baseHalfExtents, linkHalfExtents);
+    }
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void MultiBodyBaseline::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 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;
+}
+
+void MultiBodyBaseline::stepSimulation(float deltaTime)
+{
+//    getDeformableDynamicsWorld()->getMultiBodyDynamicsWorld()->stepSimulation(deltaTime);
+    m_dynamicsWorld->stepSimulation(deltaTime, 5, 1./250.);
+}
+
+
+btMultiBody* MultiBodyBaseline::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating)
+{
+    //init the base
+    btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
+    float baseMass = .1f;
+    
+    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);
+    btVector3 vel(0, 0, 0);
+    //    pMultiBody->setBaseVel(vel);
+    
+    //init the links
+    btVector3 hingeJointAxis(1, 0, 0);
+    float linkMass = .1f;
+    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
+    btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0);                      //par body's COM to cur body's COM offset
+    btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0);                         //cur body's COM to cur body's PIV offset
+    btVector3 parentComToCurrentPivot = parentComToCurrentCom - 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)
+    {
+        if (!spherical)
+            pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
+        else
+            //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
+            pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
+    }
+    
+    pMultiBody->finalizeMultiDof();
+    
+    ///
+    pWorld->addMultiBody(pMultiBody);
+    ///
+    return pMultiBody;
+}
+
+void MultiBodyBaseline::addColliders_testMultiDof(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();
+    
+    {
+        //    float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
+        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];
+        //    float pos[4]={posr.x(),posr.y(),posr.z(),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* MultiBodyBaselineCreateFunc(struct CommonExampleOptions& options)
+{
+	return new MultiBodyBaseline(options.m_guiHelper);
+}
+
+

examples/MultiBodyBaseline/MultiBodyBaseline.h

@@ -0,0 +1,20 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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 _MULTIBODY_BASELINE_H
+#define _MULTIBODY_BASELINE_H
+
+class CommonExampleInterface*  MultiBodyBaselineCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_MULTIBODY_BASELINE_H

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -233,7 +233,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
 				//				printf("error in island management\n");
 			}
 
-			btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+            btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 			if (colObj0->getIslandTag() == islandId)
 			{
 				if (colObj0->getActivationState() == ACTIVE_TAG ||
@@ -257,7 +257,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
 					//					printf("error in island management\n");
 				}
 
-				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+                btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 
 				if (colObj0->getIslandTag() == islandId)
 				{
@@ -278,7 +278,8 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
 					//					printf("error in island management\n");
 				}
 
-				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
 
 				if (colObj0->getIslandTag() == islandId)
 				{

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -17,7 +17,6 @@ subject to the following restrictions:
 #define BT_DISCRETE_DYNAMICS_WORLD_H
 
 #include "btDynamicsWorld.h"
-
 class btDispatcher;
 class btOverlappingPairCache;
 class btConstraintSolver;
@@ -26,6 +25,7 @@ class btTypedConstraint;
 class btActionInterface;
 class btPersistentManifold;
 class btIDebugDraw;
+
 struct InplaceSolverIslandCallback;
 
 #include "LinearMath/btAlignedObjectArray.h"
@@ -76,7 +76,7 @@ protected:
 
 	virtual void calculateSimulationIslands();
 
-	virtual void solveConstraints(btContactSolverInfo & solverInfo);
+	
 
 	virtual void updateActivationState(btScalar timeStep);
 
@@ -95,7 +95,7 @@ protected:
 	void serializeRigidBodies(btSerializer * serializer);
 
 	void serializeDynamicsWorldInfo(btSerializer * serializer);
-
+    
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
@@ -107,6 +107,8 @@ public:
 	///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
 	virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
 
+    virtual void solveConstraints(btContactSolverInfo & solverInfo);
+    
 	virtual void synchronizeMotionStates();
 
 	///this can be useful to synchronize a single rigid body -> graphics object
@@ -227,6 +229,16 @@ public:
 	{
 		return m_latencyMotionStateInterpolation;
 	}
+    
+    btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies()
+    {
+        return m_nonStaticRigidBodies;
+    }
+    
+    const btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies() const
+    {
+        return m_nonStaticRigidBodies;
+    }
 };
 
 #endif  //BT_DISCRETE_DYNAMICS_WORLD_H

src/BulletDynamics/Dynamics/btDynamicsWorld.h

@@ -34,7 +34,8 @@ enum btDynamicsWorldType
 	BT_CONTINUOUS_DYNAMICS_WORLD = 3,
 	BT_SOFT_RIGID_DYNAMICS_WORLD = 4,
 	BT_GPU_DYNAMICS_WORLD = 5,
-	BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6
+	BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6,
+    BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD = 7
 };
 
 ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -100,6 +100,8 @@ btMultiBody::btMultiBody(int n_links,
 	  m_baseName(0),
 	  m_basePos(0, 0, 0),
 	  m_baseQuat(0, 0, 0, 1),
+      m_basePos_interpolate(0, 0, 0),
+      m_baseQuat_interpolate(0, 0, 0, 1),
 	  m_baseMass(mass),
 	  m_baseInertia(inertia),
 
@@ -449,6 +451,16 @@ const btQuaternion &btMultiBody::getParentToLocalRot(int i) const
 	return m_links[i].m_cachedRotParentToThis;
 }
 
+const btVector3 &btMultiBody::getInterpolateRVector(int i) const
+{
+    return m_links[i].m_cachedRVector_interpolate;
+}
+
+const btQuaternion &btMultiBody::getInterpolateParentToLocalRot(int i) const
+{
+    return m_links[i].m_cachedRotParentToThis_interpolate;
+}
+
 btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
 {
 	btAssert(i >= -1);
@@ -1581,6 +1593,158 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	//printf("]\n");
 	/////////////////
 }
+void btMultiBody::predictPositionsMultiDof(btScalar dt)
+{
+    int num_links = getNumLinks();
+    // step position by adding dt * velocity
+    //btVector3 v = getBaseVel();
+    //m_basePos += dt * v;
+    //
+    btScalar *pBasePos;
+    btScalar *pBaseVel = &m_realBuf[3];  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
+    
+    // reset to current position
+    for (int i = 0; i < 3; ++i)
+    {
+        m_basePos_interpolate[i] = m_basePos[i];
+    }
+    pBasePos = m_basePos_interpolate;
+    
+    pBasePos[0] += dt * pBaseVel[0];
+    pBasePos[1] += dt * pBaseVel[1];
+    pBasePos[2] += dt * pBaseVel[2];
+    
+    ///////////////////////////////
+    //local functor for quaternion integration (to avoid error prone redundancy)
+    struct
+    {
+        //"exponential map" based on btTransformUtil::integrateTransform(..)
+        void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
+        {
+            //baseBody    =>    quat is alias and omega is global coor
+            //!baseBody    =>    quat is alibi and omega is local coor
+            
+            btVector3 axis;
+            btVector3 angvel;
+            
+            if (!baseBody)
+                angvel = quatRotate(quat, omega);  //if quat is not m_baseQuat, it is alibi => ok
+            else
+                angvel = omega;
+            
+            btScalar fAngle = angvel.length();
+            //limit the angular motion
+            if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
+            {
+                fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;
+            }
+            
+            if (fAngle < btScalar(0.001))
+            {
+                // use Taylor's expansions of sync function
+                axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle);
+            }
+            else
+            {
+                // sync(fAngle) = sin(c*fAngle)/t
+                axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle);
+            }
+            
+            if (!baseBody)
+                quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat;
+            else
+                quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5)));
+            //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
+            
+            quat.normalize();
+        }
+    } pQuatUpdateFun;
+    ///////////////////////////////
+    
+    //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
+    //
+    btScalar *pBaseQuat;
+
+    // reset to current orientation
+    for (int i = 0; i < 4; ++i)
+    {
+        m_baseQuat_interpolate[i] = m_baseQuat[i];
+    }
+    pBaseQuat = m_baseQuat_interpolate;
+
+    btScalar *pBaseOmega = &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
+    //
+    btQuaternion baseQuat;
+    baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+    btVector3 baseOmega;
+    baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+    pQuatUpdateFun(baseOmega, baseQuat, true, dt);
+    pBaseQuat[0] = baseQuat.x();
+    pBaseQuat[1] = baseQuat.y();
+    pBaseQuat[2] = baseQuat.z();
+    pBaseQuat[3] = baseQuat.w();
+
+    // Finally we can update m_jointPos for each of the m_links
+    for (int i = 0; i < num_links; ++i)
+    {
+        btScalar *pJointPos;
+        pJointPos = &m_links[i].m_jointPos_interpolate[0];
+        
+        btScalar *pJointVel = getJointVelMultiDof(i);
+        
+        switch (m_links[i].m_jointType)
+        {
+            case btMultibodyLink::ePrismatic:
+            case btMultibodyLink::eRevolute:
+            {
+                //reset to current pos
+                pJointPos[0] = m_links[i].m_jointPos[0];
+                btScalar jointVel = pJointVel[0];
+                pJointPos[0] += dt * jointVel;
+                break;
+            }
+            case btMultibodyLink::eSpherical:
+            {
+                //reset to current pos
+
+                for (int i = 0; i < 4; ++i)
+                {
+                    pJointPos[i] = m_links[i].m_jointPos[i];
+                }
+                
+                btVector3 jointVel;
+                jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+                btQuaternion jointOri;
+                jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+                pQuatUpdateFun(jointVel, jointOri, false, dt);
+                pJointPos[0] = jointOri.x();
+                pJointPos[1] = jointOri.y();
+                pJointPos[2] = jointOri.z();
+                pJointPos[3] = jointOri.w();
+                break;
+            }
+            case btMultibodyLink::ePlanar:
+            {
+                for (int i = 0; i < 3; ++i)
+                {
+                    pJointPos[i] = m_links[i].m_jointPos[i];
+                }
+                pJointPos[0] += dt * getJointVelMultiDof(i)[0];
+                
+                btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
+                btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
+                pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
+                pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
+                break;
+            }
+            default:
+            {
+            }
+        }
+        
+        m_links[i].updateInterpolationCacheMultiDof();
+    }
+}
 
 void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
 {
@@ -1589,9 +1753,9 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 	//btVector3 v = getBaseVel();
 	//m_basePos += dt * v;
 	//
-	btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
-	btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
-	//
+    btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
+    btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
+    
 	pBasePos[0] += dt * pBaseVel[0];
 	pBasePos[1] += dt * pBaseVel[1];
 	pBasePos[2] += dt * pBaseVel[2];
@@ -1645,7 +1809,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 
 	//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
 	//
-	btScalar *pBaseQuat = pq ? pq : m_baseQuat;
+    btScalar *pBaseQuat = pq ? pq : m_baseQuat;
 	btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
 	//
 	btQuaternion baseQuat;
@@ -1670,7 +1834,9 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 	// Finally we can update m_jointPos for each of the m_links
 	for (int i = 0; i < num_links; ++i)
 	{
-		btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);
+        btScalar *pJointPos;
+        pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
+        
 		btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
 
 		switch (m_links[i].m_jointType)
@@ -1678,12 +1844,14 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 			case btMultibodyLink::ePrismatic:
 			case btMultibodyLink::eRevolute:
 			{
+                //reset to current pos
 				btScalar jointVel = pJointVel[0];
 				pJointPos[0] += dt * jointVel;
 				break;
 			}
 			case btMultibodyLink::eSpherical:
 			{
+                //reset to current pos
 				btVector3 jointVel;
 				jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
 				btQuaternion jointOri;
@@ -2006,6 +2174,57 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQu
 	}
 }
 
+void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
+{
+    world_to_local.resize(getNumLinks() + 1);
+    local_origin.resize(getNumLinks() + 1);
+    
+    world_to_local[0] = getInterpolateWorldToBaseRot();
+    local_origin[0] = getInterpolateBasePos();
+    
+    if (getBaseCollider())
+    {
+        btVector3 posr = local_origin[0];
+        //    float pos[4]={posr.x(),posr.y(),posr.z(),1};
+        btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
+        btTransform tr;
+        tr.setIdentity();
+        tr.setOrigin(posr);
+        tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+        
+        getBaseCollider()->setInterpolationWorldTransform(tr);
+    }
+    
+    for (int k = 0; k < getNumLinks(); k++)
+    {
+        const int parent = getParent(k);
+        world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1];
+        local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k)));
+    }
+    
+    for (int m = 0; m < getNumLinks(); m++)
+    {
+        btMultiBodyLinkCollider *col = getLink(m).m_collider;
+        if (col)
+        {
+            int link = col->m_link;
+            btAssert(link == m);
+            
+            int index = link + 1;
+            
+            btVector3 posr = local_origin[index];
+            //            float pos[4]={posr.x(),posr.y(),posr.z(),1};
+            btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
+            btTransform tr;
+            tr.setIdentity();
+            tr.setOrigin(posr);
+            tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+            
+            col->setInterpolationWorldTransform(tr);
+        }
+    }
+}
+
 int btMultiBody::calculateSerializeBufferSize() const
 {
 	int sz = sizeof(btMultiBodyData);

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -193,12 +193,24 @@ public:
 	const btQuaternion &getWorldToBaseRot() const
 	{
 		return m_baseQuat;
-	}                                                                                               // rotates world vectors into base frame
+	}
+    
+    const btVector3 &getInterpolateBasePos() const
+    {
+        return m_basePos_interpolate;
+    }  // in world frame
+    const btQuaternion &getInterpolateWorldToBaseRot() const
+    {
+        return m_baseQuat_interpolate;
+    }
+    
+    // rotates world vectors into base frame
 	btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); }  // in world frame
 
 	void setBasePos(const btVector3 &pos)
 	{
 		m_basePos = pos;
+        m_basePos_interpolate = pos;
 	}
 
 	void setBaseWorldTransform(const btTransform &tr)
@@ -224,6 +236,7 @@ public:
 	void setWorldToBaseRot(const btQuaternion &rot)
 	{
 		m_baseQuat = rot;  //m_baseQuat asumed to ba alias!?
+        m_baseQuat_interpolate = rot;
 	}
 	void setBaseOmega(const btVector3 &omega)
 	{
@@ -273,6 +286,8 @@ public:
 
 	const btVector3 &getRVector(int i) const;              // vector from COM(parent(i)) to COM(i), in frame i's coords
 	const btQuaternion &getParentToLocalRot(int i) const;  // rotates vectors in frame parent(i) to vectors in frame i.
+    const btVector3 &getInterpolateRVector(int i) const;              // vector from COM(parent(i)) to COM(i), in frame i's coords
+    const btQuaternion &getInterpolateParentToLocalRot(int i) const;  // rotates vectors in frame parent(i) to vectors in frame i.
 
 	//
 	// transform vectors in local frame of link i to world frame (or vice versa)
@@ -421,6 +436,9 @@ public:
 
 	// timestep the positions (given current velocities).
 	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
+    
+    // predict the positions
+    void predictPositionsMultiDof(btScalar dt);
 
 	//
 	// contacts
@@ -581,6 +599,7 @@ public:
 	void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const;
 
 	void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
+    void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
 
 	virtual int calculateSerializeBufferSize() const;
 
@@ -664,7 +683,9 @@ private:
 	const char *m_baseName;                   //memory needs to be manager by user!
 
 	btVector3 m_basePos;      // position of COM of base (world frame)
+    btVector3 m_basePos_interpolate;      // position of interpolated COM of base (world frame)
 	btQuaternion m_baseQuat;  // rotates world points into base frame
+    btQuaternion m_baseQuat_interpolate;  
 
 	btScalar m_baseMass;      // mass of the base
 	btVector3 m_baseInertia;  // inertia of the base (in local frame; diagonal)

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -33,6 +33,12 @@ void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
 	m_multiBodies.remove(body);
 }
 
+void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+    btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
+    predictMultiBodyTransforms(timeStep);
+    
+}
 void btMultiBodyDynamicsWorld::calculateSimulationIslands()
 {
 	BT_PROFILE("calculateSimulationIslands");
@@ -421,292 +427,19 @@ void btMultiBodyDynamicsWorld::forwardKinematics()
 }
 void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
-	forwardKinematics();
+    solveExternalForces(solverInfo);
+    buildIslands();
+    solveInternalConstraints(solverInfo);
+}
 
-	BT_PROFILE("solveConstraints");
-
-	clearMultiBodyConstraintForces();
-
-	m_sortedConstraints.resize(m_constraints.size());
-	int i;
-	for (i = 0; i < getNumConstraints(); i++)
-	{
-		m_sortedConstraints[i] = m_constraints[i];
-	}
-	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
-	btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
-
-	m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
-	for (i = 0; i < m_multiBodyConstraints.size(); i++)
-	{
-		m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
-	}
-	m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
-
-	btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
-
-	m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
-	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
-
-#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-	{
-		BT_PROFILE("btMultiBody addForce");
-		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);
-
-				bod->addBaseForce(m_gravity * bod->getBaseMass());
-
-				for (int j = 0; j < bod->getNumLinks(); ++j)
-				{
-					bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
-				}
-			}  //if (!isSleeping)
-		}
-	}
-#endif  //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-
-	{
-		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 (!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);
-				bool doNotUpdatePos = false;
-                bool isConstraintPass = false;
-				{
-					if (!bod->isUsingRK4Integration())
-					{
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
-						m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
-						getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-					}
-					else
-					{
-						//
-						int numDofs = bod->getNumDofs() + 6;
-						int numPosVars = bod->getNumPosVars() + 7;
-						btAlignedObjectArray<btScalar> scratch_r2;
-						scratch_r2.resize(2 * numPosVars + 8 * numDofs);
-						//convenience
-						btScalar* pMem = &scratch_r2[0];
-						btScalar* scratch_q0 = pMem;
-						pMem += numPosVars;
-						btScalar* scratch_qx = pMem;
-						pMem += numPosVars;
-						btScalar* scratch_qd0 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qd1 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qd2 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qd3 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qdd0 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qdd1 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qdd2 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qdd3 = pMem;
-						pMem += numDofs;
-						btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]);
-
-						/////
-						//copy q0 to scratch_q0 and qd0 to scratch_qd0
-						scratch_q0[0] = bod->getWorldToBaseRot().x();
-						scratch_q0[1] = bod->getWorldToBaseRot().y();
-						scratch_q0[2] = bod->getWorldToBaseRot().z();
-						scratch_q0[3] = bod->getWorldToBaseRot().w();
-						scratch_q0[4] = bod->getBasePos().x();
-						scratch_q0[5] = bod->getBasePos().y();
-						scratch_q0[6] = bod->getBasePos().z();
-						//
-						for (int link = 0; link < bod->getNumLinks(); ++link)
-						{
-							for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
-								scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
-						}
-						//
-						for (int dof = 0; dof < numDofs; ++dof)
-							scratch_qd0[dof] = bod->getVelocityVector()[dof];
-						////
-						struct
-						{
-							btMultiBody* bod;
-							btScalar *scratch_qx, *scratch_q0;
-
-							void operator()()
-							{
-								for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
-									scratch_qx[dof] = scratch_q0[dof];
-							}
-						} pResetQx = {bod, scratch_qx, scratch_q0};
-						//
-						struct
-						{
-							void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size)
-							{
-								for (int i = 0; i < size; ++i)
-									pVal[i] = pCurVal[i] + dt * pDer[i];
-							}
-
-						} pEulerIntegrate;
-						//
-						struct
-						{
-							void operator()(btMultiBody* pBody, const btScalar* pData)
-							{
-								btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector());
-
-								for (int i = 0; i < pBody->getNumDofs() + 6; ++i)
-									pVel[i] = pData[i];
-							}
-						} pCopyToVelocityVector;
-						//
-						struct
-						{
-							void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size)
-							{
-								for (int i = 0; i < size; ++i)
-									pDst[i] = pSrc[start + i];
-							}
-						} pCopy;
-						//
-
-						btScalar h = solverInfo.m_timeStep;
-#define output &m_scratch_r[bod->getNumDofs()]
-						//calc qdd0 from: q0 & qd0
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
-						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-						pCopy(output, scratch_qdd0, 0, numDofs);
-						//calc q1 = q0 + h/2 * qd0
-						pResetQx();
-						bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0);
-						//calc qd1 = qd0 + h/2 * qdd0
-						pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
-						//
-						//calc qdd1 from: q1 & qd1
-						pCopyToVelocityVector(bod, scratch_qd1);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
-						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-						pCopy(output, scratch_qdd1, 0, numDofs);
-						//calc q2 = q0 + h/2 * qd1
-						pResetQx();
-						bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1);
-						//calc qd2 = qd0 + h/2 * qdd1
-						pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
-						//
-						//calc qdd2 from: q2 & qd2
-						pCopyToVelocityVector(bod, scratch_qd2);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
-						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-						pCopy(output, scratch_qdd2, 0, numDofs);
-						//calc q3 = q0 + h * qd2
-						pResetQx();
-						bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
-						//calc qd3 = qd0 + h * qdd2
-						pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
-						//
-						//calc qdd3 from: q3 & qd3
-						pCopyToVelocityVector(bod, scratch_qd3);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
-						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-						pCopy(output, scratch_qdd3, 0, numDofs);
-
-						//
-						//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
-						//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
-						btAlignedObjectArray<btScalar> delta_q;
-						delta_q.resize(numDofs);
-						btAlignedObjectArray<btScalar> delta_qd;
-						delta_qd.resize(numDofs);
-						for (int i = 0; i < numDofs; ++i)
-						{
-							delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]);
-							delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]);
-							//delta_q[i] = h*scratch_qd0[i];
-							//delta_qd[i] = h*scratch_qdd0[i];
-						}
-						//
-						pCopyToVelocityVector(bod, scratch_qd0);
-						bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
-						//
-						if (!doNotUpdatePos)
-						{
-							btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
-							pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
-
-							for (int i = 0; i < numDofs; ++i)
-								pRealBuf[i] = delta_q[i];
-
-							//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
-							bod->setPosUpdated(true);
-						}
-
-						//ugly hack which resets the cached data to t0 (needed for constraint solver)
-						{
-							for (int link = 0; link < bod->getNumLinks(); ++link)
-								bod->getLink(link).updateCacheMultiDof();
-							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m,
-							isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-							getSolverInfo().m_jointFeedbackInJointFrame);
-						}
-					}
-				}
-
-#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-				bod->clearForcesAndTorques();
-#endif         //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-			}  //if (!isSleeping)
-		}
-	}
+void btMultiBodyDynamicsWorld::buildIslands()
+{
+    m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
+}
 
+void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo)
+{
 	/// solve all the constraints for this island
-	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
-
 	m_solverMultiBodyIslandCallback->processConstraints();
 
 	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
@@ -760,11 +493,306 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 	}
 }
 
+void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo)
+{
+    forwardKinematics();
+    
+    BT_PROFILE("solveConstraints");
+    
+    clearMultiBodyConstraintForces();
+    
+    m_sortedConstraints.resize(m_constraints.size());
+    int i;
+    for (i = 0; i < getNumConstraints(); i++)
+    {
+        m_sortedConstraints[i] = m_constraints[i];
+    }
+    m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
+    btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
+    
+    m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
+    for (i = 0; i < m_multiBodyConstraints.size(); i++)
+    {
+        m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
+    }
+    m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
+    
+    btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
+    
+    m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
+    m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
+    
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+    {
+        BT_PROFILE("btMultiBody addForce");
+        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);
+                
+                bod->addBaseForce(m_gravity * bod->getBaseMass());
+                
+                for (int j = 0; j < bod->getNumLinks(); ++j)
+                {
+                    bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+                }
+            }  //if (!isSleeping)
+        }
+    }
+#endif  //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+    
+    {
+        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 (!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);
+                bool doNotUpdatePos = false;
+                bool isConstraintPass = false;
+                {
+                    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
+                    {
+                        //
+                        int numDofs = bod->getNumDofs() + 6;
+                        int numPosVars = bod->getNumPosVars() + 7;
+                        btAlignedObjectArray<btScalar> scratch_r2;
+                        scratch_r2.resize(2 * numPosVars + 8 * numDofs);
+                        //convenience
+                        btScalar* pMem = &scratch_r2[0];
+                        btScalar* scratch_q0 = pMem;
+                        pMem += numPosVars;
+                        btScalar* scratch_qx = pMem;
+                        pMem += numPosVars;
+                        btScalar* scratch_qd0 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd1 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd2 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd3 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd0 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd1 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd2 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd3 = pMem;
+                        pMem += numDofs;
+                        btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]);
+                        
+                        /////
+                        //copy q0 to scratch_q0 and qd0 to scratch_qd0
+                        scratch_q0[0] = bod->getWorldToBaseRot().x();
+                        scratch_q0[1] = bod->getWorldToBaseRot().y();
+                        scratch_q0[2] = bod->getWorldToBaseRot().z();
+                        scratch_q0[3] = bod->getWorldToBaseRot().w();
+                        scratch_q0[4] = bod->getBasePos().x();
+                        scratch_q0[5] = bod->getBasePos().y();
+                        scratch_q0[6] = bod->getBasePos().z();
+                        //
+                        for (int link = 0; link < bod->getNumLinks(); ++link)
+                        {
+                            for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
+                                scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
+                        }
+                        //
+                        for (int dof = 0; dof < numDofs; ++dof)
+                            scratch_qd0[dof] = bod->getVelocityVector()[dof];
+                        ////
+                        struct
+                        {
+                            btMultiBody* bod;
+                            btScalar *scratch_qx, *scratch_q0;
+                            
+                            void operator()()
+                            {
+                                for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
+                                    scratch_qx[dof] = scratch_q0[dof];
+                            }
+                        } pResetQx = {bod, scratch_qx, scratch_q0};
+                        //
+                        struct
+                        {
+                            void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size)
+                            {
+                                for (int i = 0; i < size; ++i)
+                                    pVal[i] = pCurVal[i] + dt * pDer[i];
+                            }
+                            
+                        } pEulerIntegrate;
+                        //
+                        struct
+                        {
+                            void operator()(btMultiBody* pBody, const btScalar* pData)
+                            {
+                                btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector());
+                                
+                                for (int i = 0; i < pBody->getNumDofs() + 6; ++i)
+                                    pVel[i] = pData[i];
+                            }
+                        } pCopyToVelocityVector;
+                        //
+                        struct
+                        {
+                            void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size)
+                            {
+                                for (int i = 0; i < size; ++i)
+                                    pDst[i] = pSrc[start + i];
+                            }
+                        } pCopy;
+                        //
+                        
+                        btScalar h = solverInfo.m_timeStep;
+#define output &m_scratch_r[bod->getNumDofs()]
+                        //calc qdd0 from: q0 & qd0
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd0, 0, numDofs);
+                        //calc q1 = q0 + h/2 * qd0
+                        pResetQx();
+                        bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0);
+                        //calc qd1 = qd0 + h/2 * qdd0
+                        pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
+                        //
+                        //calc qdd1 from: q1 & qd1
+                        pCopyToVelocityVector(bod, scratch_qd1);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd1, 0, numDofs);
+                        //calc q2 = q0 + h/2 * qd1
+                        pResetQx();
+                        bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1);
+                        //calc qd2 = qd0 + h/2 * qdd1
+                        pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
+                        //
+                        //calc qdd2 from: q2 & qd2
+                        pCopyToVelocityVector(bod, scratch_qd2);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd2, 0, numDofs);
+                        //calc q3 = q0 + h * qd2
+                        pResetQx();
+                        bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
+                        //calc qd3 = qd0 + h * qdd2
+                        pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
+                        //
+                        //calc qdd3 from: q3 & qd3
+                        pCopyToVelocityVector(bod, scratch_qd3);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd3, 0, numDofs);
+                        
+                        //
+                        //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
+                        //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
+                        btAlignedObjectArray<btScalar> delta_q;
+                        delta_q.resize(numDofs);
+                        btAlignedObjectArray<btScalar> delta_qd;
+                        delta_qd.resize(numDofs);
+                        for (int i = 0; i < numDofs; ++i)
+                        {
+                            delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]);
+                            delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]);
+                            //delta_q[i] = h*scratch_qd0[i];
+                            //delta_qd[i] = h*scratch_qdd0[i];
+                        }
+                        //
+                        pCopyToVelocityVector(bod, scratch_qd0);
+                        bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
+                        //
+                        if (!doNotUpdatePos)
+                        {
+                            btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
+                            pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
+                            
+                            for (int i = 0; i < numDofs; ++i)
+                                pRealBuf[i] = delta_q[i];
+                            
+                            //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
+                            bod->setPosUpdated(true);
+                        }
+                        
+                        //ugly hack which resets the cached data to t0 (needed for constraint solver)
+                        {
+                            for (int link = 0; link < bod->getNumLinks(); ++link)
+                                bod->getLink(link).updateCacheMultiDof();
+                            bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                      isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                      getSolverInfo().m_jointFeedbackInJointFrame);
+                        }
+                    }
+                }
+                
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+                bod->clearForcesAndTorques();
+#endif         //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+            }  //if (!isSleeping)
+        }
+    }
+}
+
+
 void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
 	btDiscreteDynamicsWorld::integrateTransforms(timeStep);
+    integrateMultiBodyTransforms(timeStep);
+}
 
-	{
+void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
+{
 		BT_PROFILE("btMultiBody stepPositions");
 		//integrate and update the Featherstone hierarchies
 
@@ -787,31 +815,61 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 				int nLinks = bod->getNumLinks();
 
 				///base + num m_links
+                if (!bod->isPosUpdated())
+                    bod->stepPositionsMultiDof(timeStep);
+                else
+                {
+                    btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
+                    pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
 
-				{
-					if (!bod->isPosUpdated())
-						bod->stepPositionsMultiDof(timeStep);
-					else
-					{
-						btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
-						pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
+                    bod->stepPositionsMultiDof(1, 0, pRealBuf);
+                    bod->setPosUpdated(false);
+                }
 
-						bod->stepPositionsMultiDof(1, 0, pRealBuf);
-						bod->setPosUpdated(false);
-					}
-				}
 
 				m_scratch_world_to_local.resize(nLinks + 1);
 				m_scratch_local_origin.resize(nLinks + 1);
-
-				bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+                bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
 			}
 			else
 			{
 				bod->clearVelocities();
 			}
 		}
-	}
+}
+
+void btMultiBodyDynamicsWorld::predictMultiBodyTransforms(btScalar timeStep)
+{
+    BT_PROFILE("btMultiBody stepPositions");
+    //integrate and update the Featherstone hierarchies
+    
+    for (int b = 0; b < m_multiBodies.size(); b++)
+    {
+        btMultiBody* bod = m_multiBodies[b];
+        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)
+        {
+            int nLinks = bod->getNumLinks();
+            bod->predictPositionsMultiDof(timeStep);
+            m_scratch_world_to_local.resize(nLinks + 1);
+            m_scratch_local_origin.resize(nLinks + 1);
+            bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+        }
+        else
+        {
+            bod->clearVelocities();
+        }
+    }
 }
 
 void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint)

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -47,7 +47,7 @@ protected:
 
 	virtual void calculateSimulationIslands();
 	virtual void updateActivationState(btScalar timeStep);
-	virtual void solveConstraints(btContactSolverInfo& solverInfo);
+	
 
 	virtual void serializeMultiBodies(btSerializer* serializer);
 
@@ -55,7 +55,8 @@ public:
 	btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration);
 
 	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);
@@ -95,7 +96,10 @@ public:
 	virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint);
 
 	virtual void integrateTransforms(btScalar timeStep);
-
+    void integrateMultiBodyTransforms(btScalar timeStep);
+    void predictMultiBodyTransforms(btScalar timeStep);
+    
+    virtual void predictUnconstraintMotion(btScalar timeStep);
 	virtual void debugDrawWorld();
 
 	virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
@@ -110,6 +114,10 @@ public:
 	virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
 	virtual void setConstraintSolver(btConstraintSolver* solver);
 	virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
+    
+    virtual void solveExternalForces(btContactSolverInfo& solverInfo);
+    virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
+    void buildIslands();
 
 };
 #endif  //BT_MULTIBODY_DYNAMICS_WORLD_H

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -111,6 +111,10 @@ struct btMultibodyLink
 
 	btQuaternion m_cachedRotParentToThis;  // rotates vectors in parent frame to vectors in local frame
 	btVector3 m_cachedRVector;             // vector from COM of parent to COM of this link, in local frame.
+    
+    // predicted verstion
+    btQuaternion m_cachedRotParentToThis_interpolate;  // rotates vectors in parent frame to vectors in local frame
+    btVector3 m_cachedRVector_interpolate;             // vector from COM of parent to COM of this link, in local frame.
 
 	btVector3 m_appliedForce;   // In WORLD frame
 	btVector3 m_appliedTorque;  // In WORLD frame
@@ -119,6 +123,7 @@ struct btMultibodyLink
 	btVector3 m_appliedConstraintTorque;  // In WORLD frame
 
 	btScalar m_jointPos[7];
+    btScalar m_jointPos_interpolate[7];
 
 	//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
 	//It gets set to zero after each internal stepSimulation call
@@ -188,42 +193,43 @@ struct btMultibodyLink
 	// routine to update m_cachedRotParentToThis and m_cachedRVector
 	void updateCacheMultiDof(btScalar *pq = 0)
 	{
-		btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
-
+        btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
+        btQuaternion& cachedRot = m_cachedRotParentToThis;
+        btVector3& cachedVector =m_cachedRVector;
 		switch (m_jointType)
 		{
 			case eRevolute:
 			{
-				m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+				cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
 
 				break;
 			}
 			case ePrismatic:
 			{
 				// m_cachedRotParentToThis never changes, so no need to update
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
+				cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
 
 				break;
 			}
 			case eSpherical:
 			{
-				m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+				cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
 			case ePlanar:
 			{
-				m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
-				m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis, m_eVector);
+				cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+				cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
 			case eFixed:
 			{
-				m_cachedRotParentToThis = m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+				cachedRot = m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
@@ -234,6 +240,57 @@ struct btMultibodyLink
 			}
 		}
 	}
+    
+    void updateInterpolationCacheMultiDof()
+    {
+        btScalar *pJointPos = &m_jointPos_interpolate[0];
+        
+        btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate;
+        btVector3& cachedVector = m_cachedRVector_interpolate;
+        switch (m_jointType)
+        {
+            case eRevolute:
+            {
+                cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+                
+                break;
+            }
+            case ePrismatic:
+            {
+                // m_cachedRotParentToThis never changes, so no need to update
+                cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
+                
+                break;
+            }
+            case eSpherical:
+            {
+                cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            case ePlanar:
+            {
+                cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+                cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            case eFixed:
+            {
+                cachedRot = m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            default:
+            {
+                //invalid type
+                btAssert(0);
+            }
+        }
+    }
 };
 
 #endif  //BT_MULTIBODY_LINK_H

src/BulletSoftBody/CMakeLists.txt

@@ -17,6 +17,11 @@ SET(BulletSoftBody_SRCS
 	btSoftSoftCollisionAlgorithm.cpp
 	btDefaultSoftBodySolver.cpp
 
+	btDeformableBackwardEulerObjective.cpp
+	btDeformableBodySolver.cpp
+	btDeformableContactProjection.cpp
+	btDeformableRigidDynamicsWorld.cpp
+
 )
 
 SET(BulletSoftBody_HDRS
@@ -33,6 +38,18 @@ SET(BulletSoftBody_HDRS
 
 	btSoftBodySolvers.h
 	btDefaultSoftBodySolver.h
+	
+	btCGProjection.h
+	btConjugateGradient.h
+	btDeformableGravityForce.h
+	btDeformableMassSpringForce.h
+	btDeformableLagrangianForce.h
+	btPreconditioner.h
+
+	btDeformableBackwardEulerObjective.h
+	btDeformableBodySolver.h
+	btDeformableContactProjection.h
+	btDeformableRigidDynamicsWorld.h
 
 	btSoftBodySolverVertexBuffer.h
 )

src/BulletSoftBody/btCGProjection.h

@@ -0,0 +1,148 @@
+/*
+ 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_CG_PROJECTION_H
+#define BT_CG_PROJECTION_H
+
+#include "btSoftBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+
+class btDeformableRigidDynamicsWorld;
+
+struct DeformableContactConstraint
+{
+    btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
+    btAlignedObjectArray<btVector3> m_direction;
+    btAlignedObjectArray<btScalar> m_value;
+    // the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve
+    btAlignedObjectArray<btScalar> m_accumulated_normal_impulse;
+    
+    DeformableContactConstraint(const btSoftBody::RContact& rcontact)
+    {
+        append(rcontact);
+    }
+    
+    DeformableContactConstraint(const btVector3 dir)
+    {
+        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_value.push_back(0);
+        m_accumulated_normal_impulse.push_back(0);
+    }
+    
+    ~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:
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    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)
+    : m_softBodies(softBodies)
+    , m_dt(dt)
+    {
+    }
+    
+    virtual ~btCGProjection()
+    {
+    }
+    
+    // apply the constraints
+    virtual void project(TVStack& x) = 0;
+    
+    virtual void setConstraints() = 0;
+    
+    // update the constraints
+    virtual void update() = 0;
+    
+    virtual void reinitialize(bool nodeUpdated)
+    {
+    }
+    
+    virtual void setWorld(btDeformableRigidDynamicsWorld* world)
+    {
+        m_world = world;
+    }
+};
+
+
+#endif /* btCGProjection_h */

src/BulletSoftBody/btConjugateGradient.h

@@ -0,0 +1,146 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_CONJUGATE_GRADIENT_H
+#define BT_CONJUGATE_GRADIENT_H
+#include <iostream>
+#include <cmath>
+#include <LinearMath/btAlignedObjectArray.h>
+#include <LinearMath/btVector3.h>
+#include "LinearMath/btQuickprof.h"
+template <class MatrixX>
+class btConjugateGradient
+{
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    TVStack r,p,z,temp;
+    int max_iterations;
+public:
+    btConjugateGradient(const int max_it_in)
+    : max_iterations(max_it_in)
+    {
+    }
+    
+    virtual ~btConjugateGradient(){}
+    
+    // return the number of iterations taken
+    int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance)
+    {
+        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;
+        // temp = A*p
+        A.multiply(p, temp);
+        
+        btScalar r_dot_z = dot(z,r), r_dot_z_new;
+        // alpha = r^T * z / (p^T * A * p)
+        btScalar alpha = r_dot_z / dot(p, temp), beta;
+        
+        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;
+            }
+            
+            // 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;
+            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;
+        return max_iterations;
+    }
+    
+    void reinitialize(const TVStack& b)
+    {
+        r.resize(b.size());
+        p.resize(b.size());
+        z.resize(b.size());
+        temp.resize(b.size());
+    }
+    
+    TVStack sub(const TVStack& a, const TVStack& b)
+    {
+        // c = a-b
+        btAssert(a.size() == b.size())
+        TVStack c;
+        c.resize(a.size());
+        for (int i = 0; i < a.size(); ++i)
+            c[i] = a[i] - b[i];
+        return c;
+    }
+    
+    btScalar squaredNorm(const TVStack& a)
+    {
+        return dot(a,a);
+    }
+    
+    btScalar dot(const TVStack& a, const TVStack& b)
+    {
+        btScalar ans(0);
+        for (int i = 0; i < a.size(); ++i)
+            ans += a[i].dot(b[i]);
+        return ans;
+    }
+    
+    void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
+    {
+//        result += s*a
+        btAssert(a.size() == result.size())
+        for (int i = 0; i < a.size(); ++i)
+            result[i] += s * a[i];
+    }
+    
+    TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
+    {
+        // result = a*s + b
+        TVStack result;
+        result.resize(a.size());
+        for (int i = 0; i < a.size(); ++i)
+            result[i] = s * a[i] + b[i];
+        return result;
+    }
+};
+#endif /* btConjugateGradient_h */

src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp

@@ -0,0 +1,147 @@
+/*
+ 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 "btDeformableBackwardEulerObjective.h"
+#include "LinearMath/btQuickprof.h"
+
+btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
+: m_softBodies(softBodies)
+, projection(m_softBodies, m_dt)
+, m_backupVelocity(backup_v)
+{
+    m_preconditioner = new DefaultPreconditioner();
+}
+
+void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt)
+{
+    BT_PROFILE("reinitialize");
+    setDt(dt);
+    if(nodeUpdated)
+    {
+        updateId();
+    }
+    for (int i = 0; i < m_lf.size(); ++i)
+    {
+        m_lf[i]->reinitialize(nodeUpdated);
+    }
+    projection.reinitialize(nodeUpdated);
+    m_preconditioner->reinitialize(nodeUpdated);
+}
+
+void btDeformableBackwardEulerObjective::setDt(btScalar dt)
+{
+    m_dt = dt;
+}
+
+void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
+{
+    BT_PROFILE("multiply");
+    // add in the mass term
+    size_t counter = 0;
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            const btSoftBody::Node& node = psb->m_nodes[j];
+            b[counter] = (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im;
+            ++counter;
+        }
+    }
+
+    for (int i = 0; i < m_lf.size(); ++i)
+    {
+        // add damping matrix
+        m_lf[i]->addScaledForceDifferential(-m_dt, x, b);
+    }
+}
+
+void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
+{
+    // only the velocity of the constrained nodes needs to be updated during CG solve
+    for (int i = 0; i < projection.m_constraints.size(); ++i)
+    {
+        int index = projection.m_constraints.getKeyAtIndex(i).getUid1();
+        m_nodes[index]->m_v = m_backupVelocity[index] + dv[index];
+    }
+}
+
+void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero)
+{
+    size_t counter = 0;
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im;
+            psb->m_nodes[j].m_v += one_over_mass * force[counter++];
+        }
+    }
+    if (setZero)
+    {
+        for (int i = 0; i < force.size(); ++i)
+            force[i].setZero();
+    }
+}
+
+void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const
+{
+    BT_PROFILE("computeResidual");
+    // add implicit force
+    for (int i = 0; i < m_lf.size(); ++i)
+    {
+        m_lf[i]->addScaledImplicitForce(dt, residual);
+    }
+}
+
+btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const
+{
+    btScalar norm_squared = 0;
+    for (int i = 0; i < residual.size(); ++i)
+    {
+        norm_squared += residual[i].length2();
+    }
+    return std::sqrt(norm_squared+SIMD_EPSILON);
+}
+
+void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
+{
+    for (int i = 0; i < m_lf.size(); ++i)
+    {
+        m_lf[i]->addScaledExplicitForce(m_dt, force);
+    }
+    applyForce(force, true);
+}
+
+void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual)
+{
+    size_t counter = 0;
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            dv[counter] = psb->m_nodes[j].m_im * residual[counter];
+            ++counter;
+        }
+    }
+}
+
+//set constraints as projections
+void btDeformableBackwardEulerObjective::setConstraints()
+{
+    // build islands for multibody solve
+    m_world->btMultiBodyDynamicsWorld::buildIslands();
+    projection.setConstraints();
+}

src/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -0,0 +1,126 @@
+/*
+ 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_BACKWARD_EULER_OBJECTIVE_H
+#define BT_BACKWARD_EULER_OBJECTIVE_H
+#include "btConjugateGradient.h"
+#include "btDeformableLagrangianForce.h"
+#include "btDeformableMassSpringForce.h"
+#include "btDeformableGravityForce.h"
+#include "btDeformableContactProjection.h"
+#include "btPreconditioner.h"
+#include "btDeformableRigidDynamicsWorld.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;
+    btDeformableContactProjection projection;
+    const TVStack& m_backupVelocity;
+    btAlignedObjectArray<btSoftBody::Node* > m_nodes;
+    btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
+    
+    virtual ~btDeformableBackwardEulerObjective() {}
+    
+    void initialize(){}
+    
+    // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
+    void computeResidual(btScalar dt, TVStack& residual) const;
+    
+    // add explicit force to the velocity
+    void applyExplicitForce(TVStack& force);
+    
+    // apply force to velocity and optionally reset the force to zero
+    void applyForce(TVStack& force, bool setZero);
+    
+    // compute the norm of the residual
+    btScalar computeNorm(const TVStack& residual) const;
+    
+    // compute one step of the solve (there is only one solve if the system is linear)
+    void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
+    
+    // perform A*x = b
+    void multiply(const TVStack& x, TVStack& b) const;
+    
+    // set initial guess for CG solve
+    void initialGuess(TVStack& dv, const TVStack& residual);
+    
+    // reset data structure and reset dt
+    void reinitialize(bool nodeUpdated, btScalar dt);
+    
+    void setDt(btScalar dt);
+    
+    // enforce constraints in CG solve
+    void enforceConstraint(TVStack& x)
+    {
+        BT_PROFILE("enforceConstraint");
+        projection.enforceConstraint(x);
+        updateVelocity(x);
+    }
+    
+    // add dv to velocity
+    void updateVelocity(const TVStack& dv);
+    
+    //set constraints as projections
+    void setConstraints();
+    
+    // update the projections and project the residual
+    void project(TVStack& r)
+    {
+        BT_PROFILE("project");
+        projection.update();
+        projection.project(r);
+    }
+    
+    // perform precondition M^(-1) x = b
+    void precondition(const TVStack& x, TVStack& b)
+    {
+        m_preconditioner->operator()(x,b);
+    }
+    
+    virtual void setWorld(btDeformableRigidDynamicsWorld* world)
+    {
+        m_world = world;
+        projection.setWorld(world);
+    }
+    
+    virtual void updateId()
+    {
+        size_t id = 0;
+        m_nodes.clear();
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                psb->m_nodes[j].index = id;
+                m_nodes.push_back(&psb->m_nodes[j]);
+                ++id;
+            }
+        }
+    }
+    
+    const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const
+    {
+        return &m_nodes;
+    }
+};
+
+#endif /* btBackwardEulerObjective_h */

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -0,0 +1,204 @@
+/*
+ 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 <stdio.h>
+#include <limits>
+#include "btDeformableBodySolver.h"
+#include "LinearMath/btQuickprof.h"
+
+btDeformableBodySolver::btDeformableBodySolver()
+: m_numNodes(0)
+, m_cg(10)
+{
+    m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
+}
+
+btDeformableBodySolver::~btDeformableBodySolver()
+{
+    delete m_objective;
+}
+
+void btDeformableBodySolver::solveConstraints(float 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);
+    
+    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);
+    m_cg.solve(*m_objective, dv, residual, tolerance);
+}
+
+void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)
+{
+    m_softBodySet.copyFromArray(softBodies);
+    bool nodeUpdated = updateNodes();
+    
+    if (nodeUpdated)
+    {
+        m_dv.resize(m_numNodes, btVector3(0,0,0));
+        m_residual.resize(m_numNodes, btVector3(0,0,0));
+        m_backupVelocity.resize(m_numNodes, btVector3(0,0,0));
+    }
+    
+    // need to setZero here as resize only set value for newly allocated items
+    for (int i = 0; i < m_numNodes; ++i)
+    {
+        m_dv[i].setZero();
+        m_residual[i].setZero();
+    }
+    
+    m_objective->reinitialize(nodeUpdated, dt);
+}
+
+void btDeformableBodySolver::setConstraints()
+{
+    BT_PROFILE("setConstraint");
+    m_objective->setConstraints();
+}
+
+void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world)
+{
+    m_objective->setWorld(world);
+}
+
+void btDeformableBodySolver::updateVelocity()
+{
+    int counter = 0;
+    for (int i = 0; i < m_softBodySet.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodySet[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter];
+            ++counter;
+        }
+    }
+}
+
+void btDeformableBodySolver::backupVelocity()
+{
+    int counter = 0;
+    for (int i = 0; i < m_softBodySet.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodySet[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
+        }
+    }
+}
+
+void btDeformableBodySolver::revertVelocity()
+{
+    int counter = 0;
+    for (int i = 0; i < m_softBodySet.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodySet[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            psb->m_nodes[j].m_v = m_backupVelocity[counter++];
+        }
+    }
+}
+
+bool btDeformableBodySolver::updateNodes()
+{
+    int numNodes = 0;
+    for (int i = 0; i < m_softBodySet.size(); ++i)
+        numNodes += m_softBodySet[i]->m_nodes.size();
+    if (numNodes != m_numNodes)
+    {
+        m_numNodes = numNodes;
+        return true;
+    }
+    return false;
+}
+
+
+void btDeformableBodySolver::predictMotion(float solverdt)
+{
+    for (int i = 0; i < m_softBodySet.size(); ++i)
+    {
+        btSoftBody *psb = m_softBodySet[i];
+        
+        if (psb->isActive())
+        {
+            // apply explicit forces to velocity
+            m_objective->applyExplicitForce(m_residual);
+            // predict motion for collision detection
+            predictDeformableMotion(psb, solverdt);
+        }
+    }
+}
+
+void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt)
+{
+    int i, ni;
+    
+    /* Prepare                */
+    psb->m_sst.sdt = dt * psb->m_cfg.timescale;
+    psb->m_sst.isdt = 1 / psb->m_sst.sdt;
+    psb->m_sst.velmrg = psb->m_sst.sdt * 3;
+    psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
+    psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
+    /* Integrate            */
+    for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
+    {
+        btSoftBody::Node& n = psb->m_nodes[i];
+        n.m_q = n.m_x;
+        n.m_x += n.m_v * dt;
+    }
+    /* Bounds                */
+    psb->updateBounds();
+    /* Nodes                */
+    ATTRIBUTE_ALIGNED16(btDbvtVolume)
+    vol;
+    for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
+    {
+        btSoftBody::Node& n = psb->m_nodes[i];
+        vol = btDbvtVolume::FromCR(n.m_x, psb->m_sst.radmrg);
+        psb->m_ndbvt.update(n.m_leaf,
+                       vol,
+                       n.m_v * psb->m_sst.velmrg,
+                       psb->m_sst.updmrg);
+    }
+
+    /* Clear contacts        */
+    psb->m_rcontacts.resize(0);
+    psb->m_scontacts.resize(0);
+    /* Optimize dbvt's        */
+    psb->m_ndbvt.optimizeIncremental(1);
+}
+
+void btDeformableBodySolver::updateSoftBodies()
+{
+    for (int i = 0; i < m_softBodySet.size(); i++)
+    {
+        btSoftBody *psb = (btSoftBody *)m_softBodySet[i];
+        if (psb->isActive())
+        {
+            psb->updateNormals(); // normal is updated here
+        }
+    }
+}

src/BulletSoftBody/btDeformableBodySolver.h

@@ -0,0 +1,94 @@
+/*
+ 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_BODY_SOLVERS_H
+#define BT_DEFORMABLE_BODY_SOLVERS_H
+
+
+#include "btSoftBodySolvers.h"
+#include "btDeformableBackwardEulerObjective.h"
+#include "btDeformableRigidDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+
+struct btCollisionObjectWrapper;
+class btDeformableBackwardEulerObjective;
+class btDeformableRigidDynamicsWorld;
+
+class btDeformableBodySolver : public btSoftBodySolver
+{
+//    using TVStack = btAlignedObjectArray<btVector3>;
+    typedef btAlignedObjectArray<btVector3> TVStack;
+protected:
+    int m_numNodes;
+    TVStack m_dv;
+    TVStack m_residual;
+    btAlignedObjectArray<btSoftBody *> m_softBodySet;
+   
+    btAlignedObjectArray<btVector3> m_backupVelocity;
+    btScalar m_dt;
+    btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;
+    
+    
+public:
+    btDeformableBackwardEulerObjective* m_objective;
+    
+    btDeformableBodySolver();
+    
+    virtual ~btDeformableBodySolver();
+    
+    virtual SolverTypes getSolverType() const
+    {
+        return DEFORMABLE_SOLVER;
+    }
+
+    virtual void updateSoftBodies();
+
+    virtual void copyBackToSoftBodies(bool bMove = true) {}
+
+    void extracted(float solverdt);
+    
+    virtual void solveConstraints(float solverdt);
+    
+    void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
+    
+    void setConstraints();
+    
+    void predictDeformableMotion(btSoftBody* psb, btScalar dt);
+    
+    void backupVelocity();
+    void revertVelocity();
+    void updateVelocity();
+    
+    bool updateNodes();
+    
+    void computeStep(TVStack& dv, const TVStack& residual);
+                     
+    virtual void predictMotion(float solverdt);
+
+    virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
+
+    virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap)
+    {
+        softBody->defaultCollisionHandler(collisionObjectWrap);
+    }
+
+    virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) {
+        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

@@ -0,0 +1,478 @@
+/*
+ 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 "btDeformableContactProjection.h"
+#include "btDeformableRigidDynamicsWorld.h"
+#include <algorithm>
+#include <cmath>
+static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
+                         btMultiBodyJacobianData& jacobianData,
+                         const btVector3& contact_point,
+                         const btVector3& dir)
+{
+    const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+    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)];
+        btAlignedObjectArray<DeformableContactConstraint>& constraints = *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)
+            {
+                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)
+                    {
+                        rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+                        va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0);
+                    }
+                    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                    {
+                        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                        if (multibodyLinkCol)
+                        {
+                            const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                            const btScalar* J_n = &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;
+                        }
+                    }
+                    
+                    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);
+                    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;
+                            friction.m_impulse[j] = -accumulated_normal*c->m_c3;
+                        }
+                        else
+                        {
+                            friction.m_static[j] = true;
+                            friction.m_impulse[j] = tangent_norm;
+                        }
+                    }
+                    else
+                    {
+                        friction.m_released[j] = true;
+                        friction.m_static[j] = false;
+                        friction.m_impulse[j] = 0;
+                        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];
+                    
+                    // the incremental impulse applied to rb in the tangential direction
+                    btVector3 incremental_tangent = (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
+                    
+
+                    // dv = new_impulse + accumulated velocity change in previous CG iterations
+                    // 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);
+                    }
+                    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                    {
+                        if (multibodyLinkCol)
+                        {
+                            // apply normal component of the impulse
+                            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
+                            if (incremental_tangent.norm() > SIMD_EPSILON)
+                            {
+                                // apply tangential component of the impulse
+                               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));
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+void btDeformableContactProjection::setConstraints()
+{
+    BT_PROFILE("setConstraints");
+    // set Dirichlet constraint
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            if (psb->m_nodes[j].m_im == 0)
+            {
+                btAlignedObjectArray<DeformableContactConstraint> c;
+                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);
+            }
+        }
+    }
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        btMultiBodyJacobianData jacobianData_normal;
+        btMultiBodyJacobianData jacobianData_complementary;
+        for (int j = 0; j < psb->m_rcontacts.size(); ++j)
+        {
+            const btSoftBody::RContact& c = psb->m_rcontacts[j];
+            // skip anchor points
+            if (c.m_node->m_im == 0)
+            {
+                continue;
+            }
+            
+            const btSoftBody::sCti& cti = c.m_cti;
+            if (cti.m_colObj->hasContactResponse())
+            {
+                btVector3 va(0, 0, 0);
+                btRigidBody* rigidCol = 0;
+                btMultiBodyLinkCollider* multibodyLinkCol = 0;
+                
+                // grab the velocity of the rigid body
+                if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                {
+                    rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0);
+                }
+                else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                {
+                    multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                    if (multibodyLinkCol)
+                    {
+                        btScalar vel = 0.0;
+                        const btScalar* jac = &c.jacobianData_normal.m_jacobians[0];
+                        const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                        for (int j = 0; j < ndof; ++j)
+                        {
+                            vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
+                        }
+                        va = cti.m_normal * 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);
+                if (dn < SIMD_EPSILON)
+                {
+                    
+                    if (m_constraints.find(c.m_node->index) == NULL)
+                    {
+                        btAlignedObjectArray<DeformableContactConstraint> constraints;
+                        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
+                        const btScalar angle_epsilon = 0.015192247; // less than 10 degree
+                        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());
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+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);
+        size_t i = m_constraints.getKeyAtIndex(index).getUid1();
+        const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
+        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];
+            for (int j = 0; j < constraints[0].m_direction.size(); ++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];
+                    }
+                }
+            }
+        }
+    }
+}
+
+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);
+        size_t i = m_constraints.getKeyAtIndex(index).getUid1();
+        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
+        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];
+        }
+        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;
+        }
+        else
+            x[i].setZero();
+        
+        // apply friction if the node is not constrained in all directions
+        if (constraints.size() < 3)
+        {
+            bool has_static_constraint = false;
+            for (int f = 0; f < frictions.size(); ++f)
+            {
+                DeformableFrictionConstraint& friction= frictions[f];
+                for (int j = 0; j < friction.m_static.size(); ++j)
+                    has_static_constraint = has_static_constraint || friction.m_static[j];
+            }
+            
+            for (int f = 0; f < frictions.size(); ++f)
+            {
+                DeformableFrictionConstraint& friction= frictions[f];
+                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];
+                    }
+                }
+            }
+        }
+    }
+}
+
+void btDeformableContactProjection::reinitialize(bool nodeUpdated)
+{
+    btCGProjection::reinitialize(nodeUpdated);
+    m_constraints.clear();
+    m_frictions.clear();
+}
+
+
+

src/BulletSoftBody/btDeformableContactProjection.h

@@ -0,0 +1,50 @@
+/*
+ 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_CONTACT_PROJECTION_H
+#define BT_CONTACT_PROJECTION_H
+#include "btCGProjection.h"
+#include "btSoftBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include "LinearMath/btHashMap.h"
+class btDeformableContactProjection : public btCGProjection
+{
+public:
+    // map from node index to constraints
+    btHashMap<btHashInt, btAlignedObjectArray<DeformableContactConstraint> > m_constraints;
+    btHashMap<btHashInt, btAlignedObjectArray<DeformableFrictionConstraint> >m_frictions;
+    
+    btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
+    : btCGProjection(softBodies, dt)
+    {
+    }
+    
+    virtual ~btDeformableContactProjection()
+    {
+    }
+    
+    // apply the constraints to the rhs
+    virtual void project(TVStack& x);
+    
+    // apply constraints to x in Ax=b
+    virtual void enforceConstraint(TVStack& x);
+    
+    // update the constraints
+    virtual void update();
+    
+    virtual void setConstraints();
+    
+    virtual void reinitialize(bool nodeUpdated);
+};
+#endif /* btDeformableContactProjection_h */

src/BulletSoftBody/btDeformableGravityForce.h

@@ -0,0 +1,68 @@
+/*
+ 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_GRAVITY_FORCE_H
+#define BT_DEFORMABLE_GRAVITY_FORCE_H
+
+#include "btDeformableLagrangianForce.h"
+
+class btDeformableGravityForce : public btDeformableLagrangianForce
+{
+public:
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    btVector3 m_gravity;
+    
+    btDeformableGravityForce(const btVector3& g) : m_gravity(g)
+    {
+    }
+    
+    virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
+    {
+    }
+    
+    virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+    {
+        addScaledGravityForce(scale, force);
+    }
+    
+    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+    {
+        
+    }
+    
+    virtual void addScaledGravityForce(btScalar scale, TVStack& force)
+    {
+        int numNodes = getNumNodes();
+        btAssert(numNodes <= force.size())
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                btSoftBody::Node& n = psb->m_nodes[j];
+                size_t id = n.index;
+                btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im;
+                btVector3 scaled_force = scale * m_gravity * mass;
+                force[id] += scaled_force;
+            }
+        }
+    }
+    
+    virtual btDeformableLagrangianForceType getForceType()
+    {
+        return BT_GRAVITY_FORCE;
+    }
+    
+    
+};
+#endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */

src/BulletSoftBody/btDeformableLagrangianForce.h

@@ -0,0 +1,72 @@
+/*
+ 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_LAGRANGIAN_FORCE_H
+#define BT_DEFORMABLE_LAGRANGIAN_FORCE_H
+
+#include "btSoftBody.h"
+#include <LinearMath/btHashMap.h>
+
+enum btDeformableLagrangianForceType
+{
+    BT_GRAVITY_FORCE = 1,
+    BT_MASSSPRING_FORCE = 2
+};
+
+class btDeformableLagrangianForce
+{
+public:
+//    using TVStack = btAlignedObjectArray<btVector3>;
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    btAlignedObjectArray<btSoftBody *> m_softBodies;
+    const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
+    
+    btDeformableLagrangianForce()
+    {
+    }
+    
+    virtual ~btDeformableLagrangianForce(){}
+    
+    virtual void addScaledImplicitForce(btScalar scale, TVStack& force) = 0;
+    
+    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
+    
+    virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
+    
+    virtual btDeformableLagrangianForceType getForceType() = 0;
+    
+    virtual void reinitialize(bool nodeUpdated)
+    {
+    }
+    
+    virtual int getNumNodes()
+    {
+        int numNodes = 0;
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            numNodes += m_softBodies[i]->m_nodes.size();
+        }
+        return numNodes;
+    }
+    
+    virtual void addSoftBody(btSoftBody* psb)
+    {
+        m_softBodies.push_back(psb);
+    }
+    
+    virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
+    {
+        m_nodes = nodes;
+    }
+};
+#endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */

src/BulletSoftBody/btDeformableMassSpringForce.h

@@ -0,0 +1,119 @@
+/*
+ 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_MASS_SPRING_H
+#define BT_MASS_SPRING_H
+
+#include "btDeformableLagrangianForce.h"
+
+class btDeformableMassSpringForce : public btDeformableLagrangianForce
+{
+public:
+//    using TVStack = btDeformableLagrangianForce::TVStack;
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    btDeformableMassSpringForce()
+    {
+    }
+    
+    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)
+    {
+        int numNodes = getNumNodes();
+        btAssert(numNodes <= force.size())
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            const btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_links.size(); ++j)
+            {
+                const btSoftBody::Link& link = psb->m_links[j];
+                btSoftBody::Node* node1 = link.m_n[0];
+                btSoftBody::Node* node2 = link.m_n[1];
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                
+                // damping force
+                btVector3 v_diff = (node2->m_v - node1->m_v);
+                btScalar k_damp = psb->m_dampingCoefficient;
+                btVector3 scaled_force = scale * v_diff * k_damp;
+                force[id1] += scaled_force;
+                force[id2] -= scaled_force;
+            }
+        }
+    }
+    
+    virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+    {
+        int numNodes = getNumNodes();
+        btAssert(numNodes <= force.size())
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            const btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_links.size(); ++j)
+            {
+                const btSoftBody::Link& link = psb->m_links[j];
+                btSoftBody::Node* node1 = link.m_n[0];
+                btSoftBody::Node* node2 = link.m_n[1];
+                btScalar kLST = link.Feature::m_material->m_kLST;
+                btScalar r = link.m_rl;
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                
+                // elastic force
+                // explicit elastic force
+                btVector3 dir = (node2->m_q - node1->m_q);
+                btVector3 dir_normalized = dir.normalized();
+                btVector3 scaled_force = scale * kLST * (dir - dir_normalized * r);
+                force[id1] += scaled_force;
+                force[id2] -= scaled_force;
+            }
+        }
+    }
+    
+    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+    {
+        // implicit damping force differential
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            const btSoftBody* psb = m_softBodies[i];
+            btScalar scaled_k_damp = psb->m_dampingCoefficient * scale;
+            for (int j = 0; j < psb->m_links.size(); ++j)
+            {
+                const btSoftBody::Link& link = psb->m_links[j];
+                btSoftBody::Node* node1 = link.m_n[0];
+                btSoftBody::Node* node2 = link.m_n[1];
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
+                df[id1] += local_scaled_df;
+                df[id2] -= local_scaled_df;
+            }
+        }
+    }
+    
+    virtual btDeformableLagrangianForceType getForceType()
+    {
+        return BT_MASSSPRING_FORCE;
+    }
+    
+};
+
+#endif /* btMassSpring_h */

src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp

@@ -0,0 +1,266 @@
+/*
+ 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/btDeformableRigidDynamicsWorld.h

@@ -0,0 +1,142 @@
+/*
+ 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_RIGID_DYNAMICS_WORLD_H
+#define BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H
+
+#include "btSoftMultiBodyDynamicsWorld.h"
+#include "btDeformableLagrangianForce.h"
+#include "btDeformableMassSpringForce.h"
+#include "btDeformableBodySolver.h"
+#include "btSoftBodyHelpers.h"
+#include <functional>
+typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
+
+class btDeformableBodySolver;
+class btDeformableLagrangianForce;
+typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
+
+class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld
+{
+    typedef btAlignedObjectArray<btVector3> TVStack;
+//    using TVStack = btAlignedObjectArray<btVector3>;
+    ///Solver classes that encapsulate multiple deformable bodies for solving
+    btDeformableBodySolver* m_deformableBodySolver;
+    btSoftBodyArray m_softBodies;
+    int m_drawFlags;
+    bool m_drawNodeTree;
+    bool m_drawFaceTree;
+    bool m_drawClusterTree;
+    btSoftBodyWorldInfo m_sbi;
+    bool m_ownsSolver;
+    btScalar m_internalTime;
+    
+    typedef void (*btSolverCallback)(btScalar time, btDeformableRigidDynamicsWorld* world);
+    btSolverCallback m_solverCallback;
+    
+protected:
+    virtual void internalSingleStepSimulation(btScalar timeStep);
+    
+    virtual void integrateTransforms(btScalar timeStep);
+    
+    void positionCorrection(btScalar dt);
+    
+    void solveDeformableBodiesConstraints(btScalar timeStep);
+    
+public:
+    btDeformableRigidDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0)
+    : btMultiBodyDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
+    m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
+    {
+        m_drawFlags = fDrawFlags::Std;
+        m_drawNodeTree = true;
+        m_drawFaceTree = false;
+        m_drawClusterTree = false;
+        m_sbi.m_broadphase = pairCache;
+        m_sbi.m_dispatcher = dispatcher;
+        m_sbi.m_sparsesdf.Initialize();
+        m_sbi.m_sparsesdf.Reset();
+        
+        m_sbi.air_density = (btScalar)1.2;
+        m_sbi.water_density = 0;
+        m_sbi.water_offset = 0;
+        m_sbi.water_normal = btVector3(0, 0, 0);
+        m_sbi.m_gravity.setValue(0, -10, 0);
+        
+        m_sbi.m_sparsesdf.Initialize();
+        m_internalTime = 0.0;
+    }
+
+    void setSolverCallback(btSolverCallback cb)
+    {
+        m_solverCallback = cb;
+    }
+    
+    virtual ~btDeformableRigidDynamicsWorld()
+    {
+    }
+    
+    virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
+    {
+        return (btMultiBodyDynamicsWorld*)(this);
+    }
+    
+    virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const
+    {
+        return (const btMultiBodyDynamicsWorld*)(this);
+    }
+    
+    virtual btDynamicsWorldType getWorldType() const
+    {
+        return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD;
+    }
+    
+    virtual void predictUnconstraintMotion(btScalar timeStep);
+    
+    virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
+    
+    btSoftBodyArray& getSoftBodyArray()
+    {
+        return m_softBodies;
+    }
+    
+    const btSoftBodyArray& getSoftBodyArray() const
+    {
+        return m_softBodies;
+    }
+    
+    btSoftBodyWorldInfo& getWorldInfo()
+    {
+        return m_sbi;
+    }
+    
+    const btSoftBodyWorldInfo& getWorldInfo() const
+    {
+        return m_sbi;
+    }
+    
+    void reinitialize(btScalar timeStep);
+    
+    void applyRigidBodyGravity(btScalar timeStep);
+    
+    void beforeSolverCallbacks(btScalar timeStep);
+    
+    void afterSolverCallbacks(btScalar timeStep);
+    
+    void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
+    
+    int getDrawFlags() const { return (m_drawFlags); }
+    void setDrawFlags(int f) { m_drawFlags = f; }
+};
+
+#endif  //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H

src/BulletSoftBody/btPreconditioner.h

@@ -0,0 +1,74 @@
+/*
+ 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_PRECONDITIONER_H
+#define BT_PRECONDITIONER_H
+
+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;
+};
+
+class DefaultPreconditioner : public Preconditioner
+{
+public:
+    virtual void operator()(const TVStack& x, TVStack& b)
+    {
+        btAssert(b.size() == x.size());
+        for (int i = 0; i < b.size(); ++i)
+            b[i] = x[i];
+    }
+    virtual void reinitialize(bool nodeUpdated)
+    {
+        
+    }
+};
+
+class MassPreconditioner : public Preconditioner
+{
+    btAlignedObjectArray<btScalar> m_inv_mass;
+    const btAlignedObjectArray<btSoftBody *>& m_softBodies;
+public:
+    MassPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies)
+    : m_softBodies(softBodies)
+    {
+    }
+    
+    virtual void reinitialize(bool nodeUpdated)
+    {
+        if (nodeUpdated)
+        {
+            m_inv_mass.clear();
+            for (int i = 0; i < m_softBodies.size(); ++i)
+            {
+                btSoftBody* psb = m_softBodies[i];
+                for (int j = 0; j < psb->m_nodes.size(); ++j)
+                    m_inv_mass.push_back(psb->m_nodes[j].m_im);
+            }
+        }
+    }
+    
+    virtual void operator()(const TVStack& x, TVStack& b)
+    {
+        btAssert(b.size() == x.size());
+        btAssert(m_inv_mass.size() == x.size());
+        for (int i = 0; i < b.size(); ++i)
+            b[i] = x[i] * m_inv_mass[i];
+    }
+};
+
+#endif /* BT_PRECONDITIONER_H */

src/BulletSoftBody/btSoftBody.cpp

@@ -110,6 +110,7 @@ void btSoftBody::initDefaults()
 
 	m_windVelocity = btVector3(0, 0, 0);
 	m_restLengthScale = btScalar(1.0);
+    m_dampingCoefficient = 1;
 }
 
 //
@@ -1757,115 +1758,115 @@ void btSoftBody::setSolver(eSolverPresets::_ preset)
 	}
 }
 
-//
 void btSoftBody::predictMotion(btScalar dt)
 {
-	int i, ni;
-
-	/* Update				*/
-	if (m_bUpdateRtCst)
-	{
-		m_bUpdateRtCst = false;
-		updateConstants();
-		m_fdbvt.clear();
-		if (m_cfg.collisions & fCollision::VF_SS)
-		{
-			initializeFaceTree();
-		}
-	}
-
-	/* Prepare				*/
-	m_sst.sdt = dt * m_cfg.timescale;
-	m_sst.isdt = 1 / m_sst.sdt;
-	m_sst.velmrg = m_sst.sdt * 3;
-	m_sst.radmrg = getCollisionShape()->getMargin();
-	m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
-	/* Forces				*/
-	addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
-	applyForces();
-	/* Integrate			*/
-	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
-	{
-		Node& n = m_nodes[i];
-		n.m_q = n.m_x;
-		btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt;
-		{
-			btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
-			btScalar clampDeltaV = maxDisplacement / m_sst.sdt;
-			for (int c = 0; c < 3; c++)
-			{
-				if (deltaV[c] > clampDeltaV)
-				{
-					deltaV[c] = clampDeltaV;
-				}
-				if (deltaV[c] < -clampDeltaV)
-				{
-					deltaV[c] = -clampDeltaV;
-				}
-			}
-		}
-		n.m_v += deltaV;
-		n.m_x += n.m_v * m_sst.sdt;
-		n.m_f = btVector3(0, 0, 0);
-	}
-	/* Clusters				*/
-	updateClusters();
-	/* Bounds				*/
-	updateBounds();
-	/* Nodes				*/
-	ATTRIBUTE_ALIGNED16(btDbvtVolume)
-	vol;
-	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
-	{
-		Node& n = m_nodes[i];
-		vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg);
-		m_ndbvt.update(n.m_leaf,
-					   vol,
-					   n.m_v * m_sst.velmrg,
-					   m_sst.updmrg);
-	}
-	/* Faces				*/
-	if (!m_fdbvt.empty())
-	{
-		for (int i = 0; i < m_faces.size(); ++i)
-		{
-			Face& f = m_faces[i];
-			const btVector3 v = (f.m_n[0]->m_v +
-								 f.m_n[1]->m_v +
-								 f.m_n[2]->m_v) /
-								3;
-			vol = VolumeOf(f, m_sst.radmrg);
-			m_fdbvt.update(f.m_leaf,
-						   vol,
-						   v * m_sst.velmrg,
-						   m_sst.updmrg);
-		}
-	}
-	/* Pose					*/
-	updatePose();
-	/* Match				*/
-	if (m_pose.m_bframe && (m_cfg.kMT > 0))
-	{
-		const btMatrix3x3 posetrs = m_pose.m_rot;
-		for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
-		{
-			Node& n = m_nodes[i];
-			if (n.m_im > 0)
-			{
-				const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com;
-				n.m_x = Lerp(n.m_x, x, m_cfg.kMT);
-			}
-		}
-	}
-	/* Clear contacts		*/
-	m_rcontacts.resize(0);
-	m_scontacts.resize(0);
-	/* Optimize dbvt's		*/
-	m_ndbvt.optimizeIncremental(1);
-	m_fdbvt.optimizeIncremental(1);
-	m_cdbvt.optimizeIncremental(1);
+    int i, ni;
+    
+    /* Update                */
+    if (m_bUpdateRtCst)
+    {
+        m_bUpdateRtCst = false;
+        updateConstants();
+        m_fdbvt.clear();
+        if (m_cfg.collisions & fCollision::VF_SS)
+        {
+            initializeFaceTree();
+        }
+    }
+    
+    /* Prepare                */
+    m_sst.sdt = dt * m_cfg.timescale;
+    m_sst.isdt = 1 / m_sst.sdt;
+    m_sst.velmrg = m_sst.sdt * 3;
+    m_sst.radmrg = getCollisionShape()->getMargin();
+    m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
+    /* Forces                */
+    addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
+    applyForces();
+    /* Integrate            */
+    for (i = 0, ni = m_nodes.size(); i < ni; ++i)
+    {
+        Node& n = m_nodes[i];
+        n.m_q = n.m_x;
+        btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt;
+        {
+            btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
+            btScalar clampDeltaV = maxDisplacement / m_sst.sdt;
+            for (int c = 0; c < 3; c++)
+            {
+                if (deltaV[c] > clampDeltaV)
+                {
+                    deltaV[c] = clampDeltaV;
+                }
+                if (deltaV[c] < -clampDeltaV)
+                {
+                    deltaV[c] = -clampDeltaV;
+                }
+            }
+        }
+        n.m_v += deltaV;
+        n.m_x += n.m_v * m_sst.sdt;
+        n.m_f = btVector3(0, 0, 0);
+    }
+    /* Clusters                */
+    updateClusters();
+    /* Bounds                */
+    updateBounds();
+    /* Nodes                */
+    ATTRIBUTE_ALIGNED16(btDbvtVolume)
+    vol;
+    for (i = 0, ni = m_nodes.size(); i < ni; ++i)
+    {
+        Node& n = m_nodes[i];
+        vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg);
+        m_ndbvt.update(n.m_leaf,
+                       vol,
+                       n.m_v * m_sst.velmrg,
+                       m_sst.updmrg);
+    }
+    /* Faces                */
+    if (!m_fdbvt.empty())
+    {
+        for (int i = 0; i < m_faces.size(); ++i)
+        {
+            Face& f = m_faces[i];
+            const btVector3 v = (f.m_n[0]->m_v +
+                                 f.m_n[1]->m_v +
+                                 f.m_n[2]->m_v) /
+            3;
+            vol = VolumeOf(f, m_sst.radmrg);
+            m_fdbvt.update(f.m_leaf,
+                           vol,
+                           v * m_sst.velmrg,
+                           m_sst.updmrg);
+        }
+    }
+    /* Pose                    */
+    updatePose();
+    /* Match                */
+    if (m_pose.m_bframe && (m_cfg.kMT > 0))
+    {
+        const btMatrix3x3 posetrs = m_pose.m_rot;
+        for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
+        {
+            Node& n = m_nodes[i];
+            if (n.m_im > 0)
+            {
+                const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com;
+                n.m_x = Lerp(n.m_x, x, m_cfg.kMT);
+            }
+        }
+    }
+    /* Clear contacts        */
+    m_rcontacts.resize(0);
+    m_scontacts.resize(0);
+    /* Optimize dbvt's        */
+    m_ndbvt.optimizeIncremental(1);
+    m_fdbvt.optimizeIncremental(1);
+    m_cdbvt.optimizeIncremental(1);
 }
 
+
 //
 void btSoftBody::solveConstraints()
 {
@@ -2261,18 +2262,45 @@ btVector3 btSoftBody::evaluateCom() const
 	return (com);
 }
 
-//
 bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
+                              const btVector3& x,
+                              btScalar margin,
+                              btSoftBody::sCti& cti) const
+{
+    btVector3 nrm;
+    const btCollisionShape* shp = colObjWrap->getCollisionShape();
+    //    const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
+    //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
+    const btTransform& wtr = colObjWrap->getWorldTransform();
+    //todo: check which transform is needed here
+    
+    btScalar dst =
+    m_worldInfo->m_sparsesdf.Evaluate(
+                                      wtr.invXform(x),
+                                      shp,
+                                      nrm,
+                                      margin);
+    if (dst < 0)
+    {
+        cti.m_colObj = colObjWrap->getCollisionObject();
+        cti.m_normal = wtr.getBasis() * nrm;
+        cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
+        return (true);
+    }
+    return (false);
+}
+//
+bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap,
 							  const btVector3& x,
 							  btScalar margin,
-							  btSoftBody::sCti& cti) const
+							  btSoftBody::sCti& cti, bool predict) const
 {
 	btVector3 nrm;
 	const btCollisionShape* shp = colObjWrap->getCollisionShape();
-	//	const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
-	//const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
-	const btTransform& wtr = colObjWrap->getWorldTransform();
-	//todo: check which transform is needed here
+    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();
 
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(
@@ -2280,13 +2308,14 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
 			shp,
 			nrm,
 			margin);
-	if (dst < 0)
+	if (!predict)
 	{
 		cti.m_colObj = colObjWrap->getCollisionObject();
 		cti.m_normal = wtr.getBasis() * nrm;
-		cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
-		return (true);
+        cti.m_offset = dst;
 	}
+    if (dst < 0)
+        return true;
 	return (false);
 }
 
@@ -2774,6 +2803,14 @@ void btSoftBody::dampClusters()
 	}
 }
 
+void btSoftBody::setSpringStiffness(btScalar k)
+{
+    for (int i = 0; i < m_links.size(); ++i)
+    {
+        m_links[i].Feature::m_material->m_kLST = k;
+    }
+}
+
 //
 void btSoftBody::Joint::Prepare(btScalar dt, int)
 {
@@ -3252,6 +3289,33 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
 			collider.ProcessColObj(this, pcoWrap);
 		}
 		break;
+        case fCollision::SDF_RD:
+        {
+            btSoftColliders::CollideSDF_RD docollide;
+            btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
+            btTransform wtr = pcoWrap->getWorldTransform();
+            
+            const btTransform ctr = pcoWrap->getWorldTransform();
+            const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
+            const btScalar basemargin = getCollisionShape()->getMargin();
+            btVector3 mins;
+            btVector3 maxs;
+            ATTRIBUTE_ALIGNED16(btDbvtVolume)
+            volume;
+            pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(),
+                                                  mins,
+                                                  maxs);
+            volume = btDbvtVolume::FromMM(mins, maxs);
+            volume.Expand(btVector3(basemargin, basemargin, basemargin));
+            docollide.psb = this;
+            docollide.m_colObj1Wrap = pcoWrap;
+            docollide.m_rigidBody = prb1;
+            
+            docollide.dynmargin = basemargin + timemargin;
+            docollide.stamargin = basemargin;
+            m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide);
+        }
+        break;
 	}
 }
 

src/BulletSoftBody/btSoftBody.h

@@ -26,7 +26,8 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
 #include "btSparseSDF.h"
 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
-
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 //#ifdef BT_USE_DOUBLE_PRECISION
 //#define btRigidBodyData	btRigidBodyDoubleData
 //#define btRigidBodyDataName	"btRigidBodyDoubleData"
@@ -159,6 +160,7 @@ public:
 			RVSmask = 0x000f,  ///Rigid versus soft mask
 			SDF_RS = 0x0001,   ///SDF based rigid vs soft
 			CL_RS = 0x0002,    ///Cluster vs convex rigid vs soft
+            SDF_RD = 0x0003,   ///DF based rigid vs deformable
 
 			SVSmask = 0x0030,  ///Rigid versus soft mask
 			VF_SS = 0x0010,    ///Vertex vs face soft vs soft handling
@@ -257,6 +259,7 @@ public:
 		btScalar m_area;     // Area
 		btDbvtNode* m_leaf;  // Leaf data
 		int m_battach : 1;   // Attached
+        int index;
 	};
 	/* Link			*/
 	ATTRIBUTE_ALIGNED16(struct)
@@ -300,6 +303,13 @@ public:
 		btScalar m_c2;     // ima*dt
 		btScalar m_c3;     // Friction
 		btScalar m_c4;     // Hardness
+        
+        // jacobians and unit impulse responses for multibody
+        btMultiBodyJacobianData jacobianData_normal;
+        btMultiBodyJacobianData jacobianData_t1;
+        btMultiBodyJacobianData jacobianData_t2;
+        btVector3 t1;
+        btVector3 t2;
 	};
 	/* SContact		*/
 	struct SContact
@@ -704,6 +714,7 @@ public:
 	btDbvt m_fdbvt;                    // Faces tree
 	btDbvt m_cdbvt;                    // Clusters tree
 	tClusterArray m_clusters;          // Clusters
+    btScalar m_dampingCoefficient;     // Damping Coefficient
 
 	btAlignedObjectArray<bool> m_clusterConnectivity;  //cluster connectivity, for self-collision
 
@@ -735,6 +746,11 @@ public:
 	{
 		return m_worldInfo;
 	}
+    
+    void setDampingCoefficient(btScalar damping_coeff)
+    {
+        m_dampingCoefficient = damping_coeff;
+    }
 
 	///@todo: avoid internal softbody shape hack and move collision code to collision library
 	virtual void setCollisionShape(btCollisionShape* collisionShape)
@@ -991,7 +1007,8 @@ public:
 				btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const;
 	void initializeFaceTree();
 	btVector3 evaluateCom() const;
-	bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
+	bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
+    bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
 	void updateNormals();
 	void updateBounds();
 	void updatePose();
@@ -1005,6 +1022,7 @@ public:
 	void solveClusters(btScalar sor);
 	void applyClusters(bool drift);
 	void dampClusters();
+    void setSpringStiffness(btScalar k);
 	void applyForces();
 	static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
 	static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);
@@ -1015,7 +1033,7 @@ public:
 	static vsolver_t getSolver(eVSolver::_ solver);
 
 	virtual int calculateSerializeBufferSize() const;
-
+  
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
 	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
 

src/BulletSoftBody/btSoftBodyInternals.h

@@ -25,7 +25,41 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 #include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 #include <string.h>  //for memset
+#include <cmath>
+static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
+                         btMultiBodyJacobianData& jacobianData,
+                         const btVector3& contact_point,
+                         const btVector3& dir)
+{
+    const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+    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;
+}
 //
 // btSymMatrix
 //
@@ -298,6 +332,46 @@ static inline btMatrix3x3 Diagonal(btScalar x)
 	m[2] = btVector3(0, 0, x);
 	return (m);
 }
+
+static inline btMatrix3x3 Diagonal(const btVector3& v)
+{
+    btMatrix3x3 m;
+    m[0] = btVector3(v.getX(), 0, 0);
+    m[1] = btVector3(0, v.getY(), 0);
+    m[2] = btVector3(0, 0, v.getZ());
+    return (m);
+}
+
+static inline btScalar Dot(const btScalar* a,const btScalar* b, int ndof)
+{
+    btScalar result = 0;
+    for (int i = 0; i < ndof; ++i)
+        result += a[i] * b[i];
+    return result;
+}
+
+static inline btMatrix3x3 OuterProduct(const btScalar* v1,const btScalar* v2,const btScalar* v3,
+                                       const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof)
+{
+    btMatrix3x3 m;
+    btScalar a11 = Dot(v1,u1,ndof);
+    btScalar a12 = Dot(v1,u2,ndof);
+    btScalar a13 = Dot(v1,u3,ndof);
+    
+    btScalar a21 = Dot(v2,u1,ndof);
+    btScalar a22 = Dot(v2,u2,ndof);
+    btScalar a23 = Dot(v2,u3,ndof);
+    
+    btScalar a31 = Dot(v3,u1,ndof);
+    btScalar a32 = Dot(v3,u2,ndof);
+    btScalar a33 = Dot(v3,u3,ndof);
+    m[0] = btVector3(a11, a12, a13);
+    m[1] = btVector3(a21, a22, a23);
+    m[2] = btVector3(a31, a32, a33);
+    return (m);
+}
+
+
 //
 static inline btMatrix3x3 Add(const btMatrix3x3& a,
 							  const btMatrix3x3& b)
@@ -854,10 +928,62 @@ struct btSoftColliders
 			psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this);
 		}
 	};
+    //
+    // CollideSDF_RS
+    //
+    struct CollideSDF_RS : btDbvt::ICollide
+    {
+        void Process(const btDbvtNode* leaf)
+        {
+            btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
+            DoNode(*node);
+        }
+        void DoNode(btSoftBody::Node& n) const
+        {
+            const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
+            btSoftBody::RContact c;
+            
+            if ((!n.m_battach) &&
+                psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
+            {
+                const btScalar ima = n.m_im;
+                const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
+                const btScalar ms = ima + imb;
+                if (ms > 0)
+                {
+                    const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
+                    static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+                    const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+                    const btVector3 ra = n.m_x - wtr.getOrigin();
+                    const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
+                    const btVector3 vb = n.m_x - n.m_q;
+                    const btVector3 vr = vb - va;
+                    const btScalar dn = btDot(vr, c.m_cti.m_normal);
+                    const btVector3 fv = vr - c.m_cti.m_normal * dn;
+                    const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
+                    c.m_node = &n;
+                    c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
+                    c.m_c1 = ra;
+                    c.m_c2 = ima * psb->m_sst.sdt;
+                    c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
+                    c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+                    psb->m_rcontacts.push_back(c);
+                    if (m_rigidBody)
+                        m_rigidBody->activate();
+                }
+            }
+        }
+        btSoftBody* psb;
+        const btCollisionObjectWrapper* m_colObj1Wrap;
+        btRigidBody* m_rigidBody;
+        btScalar dynmargin;
+        btScalar stamargin;
+    };
+
 	//
-	// CollideSDF_RS
+	// CollideSDF_RD
 	//
-	struct CollideSDF_RS : btDbvt::ICollide
+	struct CollideSDF_RD : btDbvt::ICollide
 	{
 		void Process(const btDbvtNode* leaf)
 		{
@@ -869,34 +995,75 @@ struct btSoftColliders
 			const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
 			btSoftBody::RContact c;
 
-			if ((!n.m_battach) &&
-				psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
-			{
-				const btScalar ima = n.m_im;
-				const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
-				const btScalar ms = ima + imb;
-				if (ms > 0)
-				{
-					const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
-					static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
-					const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
-					const btVector3 ra = n.m_x - wtr.getOrigin();
-					const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
-					const btVector3 vb = n.m_x - n.m_q;
-					const btVector3 vr = vb - va;
-					const btScalar dn = btDot(vr, c.m_cti.m_normal);
-					const btVector3 fv = vr - c.m_cti.m_normal * dn;
-					const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
-					c.m_node = &n;
-					c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
-					c.m_c1 = ra;
-					c.m_c2 = ima * psb->m_sst.sdt;
-					c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
-					c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
-					psb->m_rcontacts.push_back(c);
-					if (m_rigidBody)
-						m_rigidBody->activate();
-				}
+			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))
+                {
+                    const btScalar ima = n.m_im;
+                    const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
+                    const btScalar ms = ima + imb;
+                    if (ms > 0)
+                    {
+                        // resolve contact at x_n
+                        psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false);
+                        btSoftBody::sCti& cti = c.m_cti;
+                        c.m_node = &n;
+                        const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
+                        c.m_c2 = ima * psb->m_sst.sdt;
+                        c.m_c3 = fc;
+                        c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+                        
+                        if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                        {
+                            const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
+                            static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+                            const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+                            const btVector3 ra = n.m_q - wtr.getOrigin();
+                            
+                            c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
+                            c.m_c1 = ra;
+                            if (m_rigidBody)
+                                m_rigidBody->activate();
+                        }
+                        else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                        {
+                            btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                            if (multibodyLinkCol)
+                            {
+                                btVector3 normal = cti.m_normal;
+                                btVector3 t1 = generateUnitOrthogonalVector(normal);
+                                btVector3 t2 = btCross(normal, t1);
+                                btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+                                findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_q, normal);
+                                findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1);
+                                findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2);
+                                
+                                btScalar* J_n = &jacobianData_normal.m_jacobians[0];
+                                btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
+                                btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
+                                
+                                btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+                                btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+                                btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+                                
+                                btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
+                                                t1.getX(), t1.getY(), t1.getZ(),
+                                                t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
+                                const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                                btScalar dt = psb->m_sst.sdt;
+                                btMatrix3x3 local_impulse_matrix = Diagonal(1/dt) * (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+                                c.m_c0 =  rot.transpose() * local_impulse_matrix * rot;
+                                c.jacobianData_normal = jacobianData_normal;
+                                c.jacobianData_t1 = jacobianData_t1;
+                                c.jacobianData_t2 = jacobianData_t2;
+                                c.t1 = t1;
+                                c.t2 = t2;
+                            }
+                        }
+                        psb->m_rcontacts.push_back(c);
+                    }
+                }
 			}
 		}
 		btSoftBody* psb;

src/BulletSoftBody/btSoftBodySolvers.h

@@ -35,7 +35,8 @@ public:
 		CL_SOLVER,
 		CL_SIMD_SOLVER,
 		DX_SOLVER,
-		DX_SIMD_SOLVER
+		DX_SIMD_SOLVER,
+        DEFORMABLE_SOLVER
 	};
 
 protected: