Merge pull request #2467 from xhan0619/split-impulse

Split impulse

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