Merge pull request #2467 from xhan0619/split-impulse

Split impulse

examples/DeformableDemo/SplitImpulse.cpp

@@ -0,0 +1,259 @@
+/*
+ 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 "SplitImpulse.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+#define USE_SPLIT_IMPULSE 1
+///The SplitImpulse shows the effect of split impulse in deformable rigid contact.
+class SplitImpulse : public CommonRigidBodyBase
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
+public:
+	SplitImpulse(struct GUIHelperInterface* helper)
+		: CommonRigidBodyBase(helper)
+	{
+	}
+	virtual ~SplitImpulse()
+	{
+	}
+	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;
+        
+        btCollisionShape* shape[] = {
+            new btBoxShape(btVector3(1, 1, 1)),
+        };
+        btTransform startTransform;
+        startTransform.setIdentity();
+        startTransform.setOrigin(btVector3(0, 0.7, 0));
+        createRigidBody(mass, startTransform, shape[0]);
+    }
+    
+    virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
+    {
+        ///just make it a btSoftRigidDynamicsWorld please
+        ///or we will add type checking
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
+    {
+        ///just make it a btSoftRigidDynamicsWorld please
+        ///or we will add type checking
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual void renderScene()
+    {
+        CommonRigidBodyBase::renderScene();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
+            //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
+            {
+                btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+            }
+        }
+    }
+};
+
+void SplitImpulse::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)
+	btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(deformableBodySolver);
+	m_solver = sol;
+
+	m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
+//    deformableBodySolver->setWorld(getDeformableDynamicsWorld());
+	//	m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
+    btVector3 gravity = btVector3(0, -50, 0);
+	m_dynamicsWorld->setGravity(gravity);
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
+	getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
+	getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.Reset();
+    
+//    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);
+    }
+    
+#ifdef USE_SPLIT_IMPULSE
+        getDeformableDynamicsWorld()->getSolverInfo().m_erp = 0.03;
+#else
+        getDeformableDynamicsWorld()->getSolverInfo().m_erp = 0.0;
+#endif
+    
+    // create a piece of cloth
+    {
+        const btScalar s = 4;
+        const btScalar h = 0;
+        
+        btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
+                                                         btVector3(+s, h, -s),
+                                                         btVector3(-s, h, +s),
+                                                         btVector3(+s, h, +s),
+//                                                         3,3,
+                                                          20,20,
+                                                          1 + 2 + 4 + 8, true);
+//                                                          0, true);
+
+        
+        psb->getCollisionShape()->setMargin(0.15);
+        psb->generateBendingConstraints(2);
+        psb->setTotalMass(1);
+        psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        psb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        psb->m_cfg.kDF = 2;
+        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+        
+        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30,1, true);
+        getDeformableDynamicsWorld()->addForce(psb, mass_spring);
+        m_forces.push_back(mass_spring);
+        
+        btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(gravity);
+        getDeformableDynamicsWorld()->addForce(psb, gravity_force);
+        m_forces.push_back(gravity_force);
+        // add a few rigid bodies
+        Ctor_RbUpStack(1);
+    }
+    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void SplitImpulse::exitPhysics()
+{
+	//cleanup in the reverse order of creation/initialization
+
+	//remove the rigidbodies from the dynamics world and delete them
+	int i;
+	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+	{
+		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+		btRigidBody* body = btRigidBody::upcast(obj);
+		if (body && body->getMotionState())
+		{
+			delete body->getMotionState();
+		}
+		m_dynamicsWorld->removeCollisionObject(obj);
+		delete obj;
+	}
+    // delete forces
+    for (int j = 0; j < m_forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = m_forces[j];
+        delete force;
+    }
+    m_forces.clear();
+	//delete collision shapes
+	for (int j = 0; j < m_collisionShapes.size(); j++)
+	{
+		btCollisionShape* shape = m_collisionShapes[j];
+		delete shape;
+	}
+	m_collisionShapes.clear();
+
+	delete m_dynamicsWorld;
+
+	delete m_solver;
+
+	delete m_broadphase;
+
+	delete m_dispatcher;
+
+	delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* SplitImpulseCreateFunc(struct CommonExampleOptions& options)
+{
+	return new SplitImpulse(options.m_guiHelper);
+}
+
+

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

examples/ExampleBrowser/CMakeLists.txt

@@ -369,6 +369,8 @@ SET(BulletExampleBrowser_SRCS
   ../DeformableDemo/DeformableMultibody.h
   ../DeformableDemo/DeformableRigid.cpp
   ../DeformableDemo/DeformableRigid.h
+  ../DeformableDemo/SplitImpulse.cpp
+  ../DeformableDemo/SplitImpulse.h
   ../DeformableDemo/VolumetricDeformable.cpp
   ../DeformableDemo/VolumetricDeformable.h
   ../DeformableDemo/DeformableClothAnchor.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -45,6 +45,7 @@
 #include "../DynamicControlDemo/MotorDemo.h"
 #include "../RollingFrictionDemo/RollingFrictionDemo.h"
 #include "../DeformableDemo/DeformableRigid.h"
+#include "../DeformableDemo/SplitImpulse.h"
 #include "../DeformableDemo/ClothFriction.h"
 #include "../DeformableDemo/Pinch.h"
 #include "../DeformableDemo/DeformableSelfCollision.h"
@@ -197,6 +198,7 @@ static ExampleEntry gDefaultExamples[] =
         ExampleEntry(1, "Cloth Friction", "Cloth friction contact", ClothFrictionCreateFunc),
         ExampleEntry(1, "Deformable-Deformable Friction Contact", "Deformable friction contact", PinchFrictionCreateFunc),
         ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableRigidCreateFunc),
+        ExampleEntry(1, "Split Impulse Contact", "Split impulse test", SplitImpulseCreateFunc),
         ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc),
         ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc),
         ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc),

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -332,6 +332,48 @@ public:
 			}
 		}
 	}
+    
+    void applyPushImpulse(const btVector3& impulse, const btVector3& rel_pos)
+    {
+        if (m_inverseMass != btScalar(0.))
+        {
+            applyCentralPushImpulse(impulse);
+            if (m_angularFactor)
+            {
+                applyTorqueTurnImpulse(rel_pos.cross(impulse * m_linearFactor));
+            }
+        }
+    }
+    
+    btVector3 getPushVelocity()
+    {
+        return m_pushVelocity;
+    }
+    
+    btVector3 getTurnVelocity()
+    {
+        return m_turnVelocity;
+    }
+    
+    void setPushVelocity(const btVector3& v)
+    {
+        m_pushVelocity = v;
+    }
+    
+    void setTurnVelocity(const btVector3& v)
+    {
+        m_turnVelocity = v;
+    }
+    
+    void applyCentralPushImpulse(const btVector3& impulse)
+    {
+        m_pushVelocity += impulse * m_linearFactor * m_inverseMass;
+    }
+    
+    void applyTorqueTurnImpulse(const btVector3& torque)
+    {
+        m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+    }
 
 	void clearForces()
 	{

src/BulletSoftBody/CMakeLists.txt

@@ -47,6 +47,7 @@ SET(BulletSoftBody_HDRS
 	btDeformableMassSpringForce.h
 	btDeformableCorotatedForce.h
 	btDeformableNeoHookeanForce.h
+	btDeformableLinearElasticityForce.h
 	btDeformableLagrangianForce.h
 	btPreconditioner.h
 

src/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -20,6 +20,7 @@
 #include "btDeformableMassSpringForce.h"
 #include "btDeformableGravityForce.h"
 #include "btDeformableCorotatedForce.h"
+#include "btDeformableLinearElasticityForce.h"
 #include "btDeformableNeoHookeanForce.h"
 #include "btDeformableContactProjection.h"
 #include "btPreconditioner.h"

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -36,7 +36,7 @@ btDeformableBodySolver::~btDeformableBodySolver()
 
 void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
 {
-    BT_PROFILE("solveConstraints");
+    BT_PROFILE("solveDeformableConstraints");
     if (!m_implicit)
     {
         m_objective->computeResidual(solverdt, m_residual);
@@ -241,6 +241,16 @@ btScalar btDeformableBodySolver::solveContactConstraints()
     return maxSquaredResidual;
 }
 
+btScalar btDeformableBodySolver::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
+{
+    BT_PROFILE("solveSplitImpulse");
+    return m_objective->m_projection.solveSplitImpulse(infoGlobal);
+}
+
+void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
+{
+     m_objective->m_projection.splitImpulseSetup(infoGlobal);
+}
 
 void btDeformableBodySolver::updateVelocity()
 {
@@ -324,7 +334,7 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit)
             }
             else
                 m_dv[counter] =  psb->m_nodes[j].m_v - m_backupVelocity[counter];
-            psb->m_nodes[j].m_v = m_backupVelocity[counter];
+            psb->m_nodes[j].m_v = m_backupVelocity[counter] + psb->m_nodes[j].m_vsplit;
             ++counter;
         }
     }

src/BulletSoftBody/btDeformableBodySolver.h

@@ -66,6 +66,12 @@ public:
     
     // solve the contact between deformable and rigid as well as among deformables
     btScalar solveContactConstraints();
+    
+    // solve the position error  between deformable and rigid as well as among deformables;
+    btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
+    
+    // set up the position error in split impulse
+    void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
 
     // resize/clear data structures
     void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);

src/BulletSoftBody/btDeformableContactConstraint.cpp

@@ -140,11 +140,14 @@ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btS
 {
     m_total_normal_dv.setZero();
     m_total_tangent_dv.setZero();
+    // penetration is non-positive. The magnitude of penetration is the depth of penetration.
+    m_penetration = btMin(btScalar(0), c.m_cti.m_offset);
 }
 
 btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
 : m_contact(other.m_contact)
 , btDeformableContactConstraint(other)
+, m_penetration(other.m_penetration)
 {
     m_total_normal_dv = other.m_total_normal_dv;
     m_total_tangent_dv = other.m_total_tangent_dv;
@@ -285,6 +288,36 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
     return residualSquare;
 }
 
+btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
+{
+    const btSoftBody::sCti& cti = m_contact->m_cti;
+    const btScalar dn = m_penetration;
+    if (dn != 0)
+    {
+        const btVector3 impulse = (m_contact->m_c0 * (cti.m_normal * dn / infoGlobal.m_timeStep));
+        // one iteration of the position impulse corrects all the position error at this timestep
+        m_penetration -= dn;
+        // apply impulse to deformable nodes involved and change their position
+        applySplitImpulse(impulse);
+        // apply impulse to the rigid/multibodies involved and change their position
+        if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+        {
+            btRigidBody* rigidCol = 0;
+            rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+            if (rigidCol)
+            {
+                rigidCol->applyPushImpulse(impulse, m_contact->m_c1);
+            }
+        }
+        else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+        {
+            // todo xuchenhan@
+        }
+        return (m_penetration/infoGlobal.m_timeStep) * (m_penetration/infoGlobal.m_timeStep);
+    }
+    return 0;
+}
+
 /* ================   Node vs. Rigid   =================== */
 btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact)
     : m_node(contact.m_node)
@@ -316,6 +349,13 @@ void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impul
     contact->m_node->m_v -= dv;
 }
 
+void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
+{
+    const btSoftBody::DeformableNodeRigidContact* contact = getContact();
+    btVector3 dv = impulse * contact->m_c2;
+    contact->m_node->m_vsplit -= dv;
+};
+
 /* ================   Face vs. Rigid   =================== */
 btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
 : m_face(contact.m_face)
@@ -386,6 +426,26 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
     v2 += dv2;
 }
 
+void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
+{
+    const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+    btVector3 dv = impulse * contact->m_c2;
+    btSoftBody::Face* face = contact->m_face;
+
+    btVector3& v0 = face->m_n[0]->m_vsplit;
+    btVector3& v1 = face->m_n[1]->m_vsplit;
+    btVector3& v2 = face->m_n[2]->m_vsplit;
+    const btScalar& im0 = face->m_n[0]->m_im;
+    const btScalar& im1 = face->m_n[1]->m_im;
+    const btScalar& im2 = face->m_n[2]->m_im;
+    if (im0 > 0)
+        v0 -= dv * contact->m_weights[0];
+    if (im1 > 0)
+        v1 -= dv * contact->m_weights[1];
+    if (im2 > 0)
+        v2 -= dv * contact->m_weights[2];
+}
+
 /* ================   Face vs. Node   =================== */
 btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact)
 : m_node(contact.m_node)

src/BulletSoftBody/btDeformableContactConstraint.h

@@ -50,6 +50,9 @@ public:
     // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact
     virtual btScalar solveConstraint() = 0;
     
+    // solve the position error by applying an inelastic impulse that changes only the position (not velocity)
+    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) = 0;
+    
     // get the velocity of the object A in the contact
     virtual btVector3 getVa() const = 0;
     
@@ -61,6 +64,12 @@ public:
     
     // apply impulse to the soft body node and/or face involved
     virtual void applyImpulse(const btVector3& impulse) = 0;
+    
+    // apply position based impulse to the soft body node and/or face involved
+    virtual void applySplitImpulse(const btVector3& impulse) = 0;
+    
+    // scale the penetration depth by erp
+    virtual void setPenetrationScale(btScalar scale) = 0;
 };
 
 //
@@ -90,6 +99,11 @@ public:
         return 0;
     }
     
+    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
+    {
+        return 0;
+    }
+
     virtual btVector3 getVa() const
     {
         return btVector3(0,0,0);
@@ -106,6 +120,8 @@ public:
     }
     
     virtual void applyImpulse(const btVector3& impulse){}
+    virtual void applySplitImpulse(const btVector3& impulse){}
+    virtual void setPenetrationScale(btScalar scale){}
 };
 
 //
@@ -122,6 +138,11 @@ public:
     {
     }
     virtual btScalar solveConstraint();
+    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
+    {
+        // todo xuchenhan@
+        return 0;
+    }
     // object A is the rigid/multi body, and object B is the deformable node/face
     virtual btVector3 getVa() const;
     // get the velocity of the deformable node in contact
@@ -131,6 +152,11 @@ public:
         return btVector3(0,0,0);
     }
     virtual void applyImpulse(const btVector3& impulse);
+    virtual void applySplitImpulse(const btVector3& impulse)
+    {
+        // todo xuchenhan@
+    };
+    virtual void setPenetrationScale(btScalar scale){}
 };
 
 
@@ -141,6 +167,7 @@ class btDeformableRigidContactConstraint : public btDeformableContactConstraint
 public:
     btVector3 m_total_normal_dv;
     btVector3 m_total_tangent_dv;
+    btScalar m_penetration;
     const btSoftBody::DeformableRigidContact* m_contact;
     
     btDeformableRigidContactConstraint(){}
@@ -154,6 +181,13 @@ public:
     virtual btVector3 getVa() const;
     
     virtual btScalar solveConstraint();
+    
+    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
+    
+    virtual void setPenetrationScale(btScalar scale)
+    {
+        m_penetration *= scale;
+    }
 };
 
 //
@@ -185,6 +219,7 @@ public:
     }
     
     virtual void applyImpulse(const btVector3& impulse);
+    virtual void applySplitImpulse(const btVector3& impulse);
 };
 
 //
@@ -214,6 +249,7 @@ public:
     }
     
     virtual void applyImpulse(const btVector3& impulse);
+    virtual void applySplitImpulse(const btVector3& impulse);
 };
 
 //
@@ -235,6 +271,12 @@ public:
     
     virtual btScalar solveConstraint();
     
+    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
+    {
+        // todo: xuchenhan@
+        return 0;
+    }
+    
     // get the velocity of the object A in the contact
     virtual btVector3 getVa() const;
     
@@ -251,5 +293,10 @@ public:
     }
     
     virtual void applyImpulse(const btVector3& impulse);
+    virtual void applySplitImpulse(const btVector3& impulse)
+    {
+        // todo xuchenhan@
+    }
+    virtual void setPenetrationScale(btScalar scale){}
 };
 #endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */

src/BulletSoftBody/btDeformableContactProjection.cpp

@@ -51,6 +51,57 @@ btScalar btDeformableContactProjection::update()
     return residualSquare;
 }
 
+void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
+{
+    // node constraints
+    for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
+    {
+        btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index);
+        for (int i = 0; i < constraints.size(); ++i)
+        {
+            constraints[i].setPenetrationScale(infoGlobal.m_erp);
+        }
+    }
+    
+    // face constraints
+    for (int index = 0; index < m_allFaceConstraints.size(); ++index)
+    {
+        btDeformableContactConstraint* constraint = m_allFaceConstraints[index];
+        constraint->setPenetrationScale(infoGlobal.m_erp);
+    }
+}
+
+btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
+{
+    btScalar residualSquare = 0;
+    // node constraints
+    for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
+    {
+        btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index);
+        for (int i = 0; i < constraints.size(); ++i)
+        {
+            btScalar localResidualSquare = constraints[i].solveSplitImpulse(infoGlobal);
+            residualSquare = btMax(residualSquare, localResidualSquare);
+        }
+    }
+    
+    // anchor constraints
+    for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index)
+    {
+        btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index);
+        btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
+        residualSquare = btMax(residualSquare, localResidualSquare);
+    }
+    
+    // face constraints
+    for (int index = 0; index < m_allFaceConstraints.size(); ++index)
+    {
+        btDeformableContactConstraint* constraint = m_allFaceConstraints[index];
+        btScalar localResidualSquare = constraint->solveSplitImpulse(infoGlobal);
+        residualSquare = btMax(residualSquare, localResidualSquare);
+    }
+    return residualSquare;
+}
 
 void btDeformableContactProjection::setConstraints()
 {

src/BulletSoftBody/btDeformableContactProjection.h

@@ -60,9 +60,12 @@ public:
     // add friction force to the rhs of the linear solve
     virtual void applyDynamicFriction(TVStack& f);
     
-    // update the constraints
+    // update and solve the constraints
     virtual btScalar update();
     
+    // solve the position error using split impulse
+    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
+    
     // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
     virtual void setConstraints();
     
@@ -70,5 +73,7 @@ public:
     virtual void setProjection();
     
     virtual void reinitialize(bool nodeUpdated);
+    
+    virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
 };
 #endif /* btDeformableContactProjection_h */

src/BulletSoftBody/btDeformableLagrangianForce.h

@@ -25,7 +25,8 @@ enum btDeformableLagrangianForceType
     BT_GRAVITY_FORCE = 1,
     BT_MASSSPRING_FORCE = 2,
     BT_COROTATED_FORCE = 3,
-    BT_NEOHOOKEAN_FORCE = 4
+    BT_NEOHOOKEAN_FORCE = 4,
+    BT_LINEAR_ELASTICITY_FORCE = 5
 };
 
 static inline double randomDouble(double low, double high)

src/BulletSoftBody/btDeformableLinearElasticityForce.h

@@ -0,0 +1,340 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_LINEAR_ELASTICITY_H
+#define BT_LINEAR_ELASTICITY_H
+
+#include "btDeformableLagrangianForce.h"
+#include "LinearMath/btQuickprof.h"
+class btDeformableLinearElasticityForce : public btDeformableLagrangianForce
+{
+public:
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    btScalar m_mu, m_lambda;
+    btScalar m_mu_damp, m_lambda_damp;
+    btDeformableLinearElasticityForce(): m_mu(1), m_lambda(1)
+    {
+        btScalar damping = 0.05;
+        m_mu_damp = damping * m_mu;
+        m_lambda_damp = damping * m_lambda;
+    }
+    
+    btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda)
+    {
+        m_mu_damp = damping * m_mu;
+        m_lambda_damp = damping * m_lambda;
+    }
+    
+    virtual void addScaledForces(btScalar scale, TVStack& force)
+    {
+        addScaledDampingForce(scale, force);
+        addScaledElasticForce(scale, force);
+    }
+    
+    virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+    {
+        addScaledElasticForce(scale, force);
+    }
+    
+    // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
+    virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+    {
+        if (m_mu_damp == 0 && m_lambda_damp == 0)
+            return;
+        int numNodes = getNumNodes();
+        btAssert(numNodes <= force.size());
+        btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
+            for (int j = 0; j < psb->m_tetras.size(); ++j)
+            {
+                btSoftBody::Tetra& tetra = psb->m_tetras[j];
+                btSoftBody::Node* node0 = tetra.m_n[0];
+                btSoftBody::Node* node1 = tetra.m_n[1];
+                btSoftBody::Node* node2 = tetra.m_n[2];
+                btSoftBody::Node* node3 = tetra.m_n[3];
+                size_t id0 = node0->index;
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                size_t id3 = node3->index;
+                btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
+                btMatrix3x3 I;
+                I.setIdentity();
+                btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
+                //                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
+                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+                btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+                
+                // damping force differential
+                btScalar scale1 = scale * tetra.m_element_measure;
+                force[id0] -= scale1 * df_on_node0;
+                force[id1] -= scale1 * df_on_node123.getColumn(0);
+                force[id2] -= scale1 * df_on_node123.getColumn(1);
+                force[id3] -= scale1 * df_on_node123.getColumn(2);
+            }
+        }
+    }
+    
+    virtual double totalElasticEnergy(btScalar dt)
+    {
+        double energy = 0;
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
+            for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
+            {
+                btSoftBody::Tetra& tetra = psb->m_tetras[j];
+                btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
+                energy += tetra.m_element_measure * elasticEnergyDensity(s);
+            }
+        }
+        return energy;
+    }
+    
+    // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
+    virtual double totalDampingEnergy(btScalar dt)
+    {
+        double energy = 0;
+        int sz = 0;
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                sz = btMax(sz, psb->m_nodes[j].index);
+            }
+        }
+        TVStack dampingForce;
+        dampingForce.resize(sz+1);
+        for (int i = 0; i < dampingForce.size(); ++i)
+            dampingForce[i].setZero();
+        addScaledDampingForce(0.5, dampingForce);
+        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];
+                energy -= dampingForce[node.index].dot(node.m_v) / dt;
+            }
+        }
+        return energy;
+    }
+    
+    double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
+    {
+        double density = 0;
+        btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
+        btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
+        density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2());
+        density += m_lambda * trace * trace * 0.5;
+        return density;
+    }
+    
+    virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+    {
+        int numNodes = getNumNodes();
+        btAssert(numNodes <= force.size());
+        btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
+            btScalar max_p = psb->m_cfg.m_maxStress;
+            for (int j = 0; j < psb->m_tetras.size(); ++j)
+            {
+                btSoftBody::Tetra& tetra = psb->m_tetras[j];
+                btMatrix3x3 P;
+                firstPiola(psb->m_tetraScratches[j],P);
+#if USE_SVD
+                if (max_p > 0)
+                {
+                    // since we want to clamp the principal stress to max_p, we only need to
+                    // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
+                    btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
+                    if (trPTP > max_p * max_p)
+                    {
+                        btMatrix3x3 U, V;
+                        btVector3 sigma;
+                        singularValueDecomposition(P, U, sigma, V);
+                        sigma[0] = btMin(sigma[0], max_p);
+                        sigma[1] = btMin(sigma[1], max_p);
+                        sigma[2] = btMin(sigma[2], max_p);
+                        sigma[0] = btMax(sigma[0], -max_p);
+                        sigma[1] = btMax(sigma[1], -max_p);
+                        sigma[2] = btMax(sigma[2], -max_p);
+                        btMatrix3x3 Sigma;
+                        Sigma.setIdentity();
+                        Sigma[0][0] = sigma[0];
+                        Sigma[1][1] = sigma[1];
+                        Sigma[2][2] = sigma[2];
+                        P = U * Sigma * V.transpose();
+                    }
+                }
+#endif
+                //                btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+                btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
+                btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
+                
+                btSoftBody::Node* node0 = tetra.m_n[0];
+                btSoftBody::Node* node1 = tetra.m_n[1];
+                btSoftBody::Node* node2 = tetra.m_n[2];
+                btSoftBody::Node* node3 = tetra.m_n[3];
+                size_t id0 = node0->index;
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                size_t id3 = node3->index;
+                
+                // elastic force
+                btScalar scale1 = scale * tetra.m_element_measure;
+                force[id0] -= scale1 * force_on_node0;
+                force[id1] -= scale1 * force_on_node123.getColumn(0);
+                force[id2] -= scale1 * force_on_node123.getColumn(1);
+                force[id3] -= scale1 * force_on_node123.getColumn(2);
+            }
+        }
+    }
+    
+    // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
+    virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+    {
+        if (m_mu_damp == 0 && m_lambda_damp == 0)
+            return;
+        int numNodes = getNumNodes();
+        btAssert(numNodes <= df.size());
+        btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
+            for (int j = 0; j < psb->m_tetras.size(); ++j)
+            {
+                btSoftBody::Tetra& tetra = psb->m_tetras[j];
+                btSoftBody::Node* node0 = tetra.m_n[0];
+                btSoftBody::Node* node1 = tetra.m_n[1];
+                btSoftBody::Node* node2 = tetra.m_n[2];
+                btSoftBody::Node* node3 = tetra.m_n[3];
+                size_t id0 = node0->index;
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                size_t id3 = node3->index;
+                btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
+                btMatrix3x3 I;
+                I.setIdentity();
+                btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
+                //                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
+                //                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+                btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+                btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
+                
+                // damping force differential
+                btScalar scale1 = scale * tetra.m_element_measure;
+                df[id0] -= scale1 * df_on_node0;
+                df[id1] -= scale1 * df_on_node123.getColumn(0);
+                df[id2] -= scale1 * df_on_node123.getColumn(1);
+                df[id3] -= scale1 * df_on_node123.getColumn(2);
+            }
+        }
+    }
+    
+    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+    {
+        int numNodes = getNumNodes();
+        btAssert(numNodes <= df.size());
+        btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
+            for (int j = 0; j < psb->m_tetras.size(); ++j)
+            {
+                btSoftBody::Tetra& tetra = psb->m_tetras[j];
+                btSoftBody::Node* node0 = tetra.m_n[0];
+                btSoftBody::Node* node1 = tetra.m_n[1];
+                btSoftBody::Node* node2 = tetra.m_n[2];
+                btSoftBody::Node* node3 = tetra.m_n[3];
+                size_t id0 = node0->index;
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                size_t id3 = node3->index;
+                btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
+                btMatrix3x3 dP;
+                firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
+                //                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+                btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+                btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
+                
+                // elastic force differential
+                btScalar scale1 = scale * tetra.m_element_measure;
+                df[id0] -= scale1 * df_on_node0;
+                df[id1] -= scale1 * df_on_node123.getColumn(0);
+                df[id2] -= scale1 * df_on_node123.getColumn(1);
+                df[id3] -= scale1 * df_on_node123.getColumn(2);
+            }
+        }
+    }
+    
+    void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
+    {
+        btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
+        btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
+        P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
+    }
+    
+    // Let P be the first piola stress.
+    // This function calculates the dP = dP/dF * dF
+    void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF,  btMatrix3x3& dP)
+    {
+        btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
+        dP = (dF + dF.transpose()) * m_mu +  btMatrix3x3::getIdentity()  * m_lambda * trace;
+    }
+    
+    // Let Q be the damping stress.
+    // This function calculates the dP = dQ/dF * dF
+    void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF,  btMatrix3x3& dP)
+    {
+        btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
+        dP = (dF + dF.transpose()) * m_mu_damp +  btMatrix3x3::getIdentity()  * m_lambda_damp * trace;
+    }
+    
+    virtual btDeformableLagrangianForceType getForceType()
+    {
+        return BT_LINEAR_ELASTICITY_FORCE;
+    }
+    
+};
+#endif /* BT_LINEAR_ELASTICITY_H */

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -104,3 +104,40 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS
         }
     }
 }
+
+void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+    BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
+    int iteration;
+    if (infoGlobal.m_splitImpulse)
+    {
+        {
+            m_deformableSolver->splitImpulseSetup(infoGlobal);
+            for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
+            {
+                btScalar leastSquaresResidual = 0.f;
+                {
+                    int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                    int j;
+                    for (j = 0; j < numPoolConstraints; j++)
+                    {
+                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                        
+                        btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+                        leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+                    }
+                    // solve the position correction between deformable and rigid/multibody
+                    btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
+                    leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+                }
+                if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
+                {
+#ifdef VERBOSE_RESIDUAL_PRINTF
+                    printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
+#endif
+                    break;
+                }
+            }
+        }
+    }
+}

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h

@@ -44,6 +44,8 @@ protected:
     // write the velocity of the underlying rigid body to the the the solver body
     void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
     
+    virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+    
 public:
     BT_DECLARE_ALIGNED_ALLOCATOR();
     

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -118,9 +118,32 @@ void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
     }
 }
 
+void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep)
+{
+    // correct the position of rigid bodies with temporary velocity generated from split impulse
+    btContactSolverInfo infoGlobal;
+    btVector3 zero(0,0,0);
+    for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
+    {
+        btRigidBody* rb = m_nonStaticRigidBodies[i];
+        //correct the position/orientation based on push/turn recovery
+        btTransform newTransform;
+        btVector3 pushVelocity = rb->getPushVelocity();
+        btVector3 turnVelocity = rb->getTurnVelocity();
+        if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0)
+        {
+            btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform);
+            rb->setWorldTransform(newTransform);
+            rb->setPushVelocity(zero);
+            rb->setTurnVelocity(zero);
+        }
+    }
+}
+
 void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
     BT_PROFILE("integrateTransforms");
+    positionCorrection(timeStep);
     btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
@@ -142,6 +165,8 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
                 }
             }
             node.m_x  =  node.m_x + timeStep * node.m_v;
+            node.m_v -= node.m_vsplit;
+            node.m_vsplit.setZero();
             node.m_q = node.m_x;
             node.m_vn = node.m_v;
         }

src/BulletSoftBody/btSoftBody.h

@@ -257,6 +257,7 @@ public:
 		btVector3 m_x;       // Position
 		btVector3 m_q;       // Previous step position/Test position
 		btVector3 m_v;       // Velocity
+        btVector3 m_vsplit;  // Temporary Velocity in addintion to velocity used in split impulse
         btVector3 m_vn;      // Previous step velocity
 		btVector3 m_f;       // Force accumulator
 		btVector3 m_n;       // Normal