switch explicit elastic force

13 files changed, 193 insertions, 77 deletions

diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp
index e8c4c3edc..3f27df5a9 100644
--- a/src/BulletSoftBody/btBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp
@@ -57,9 +57,6 @@ void btBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
     {
         // add damping matrix
         m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
-        
-        // add stiffness matrix when fully implicity
-        m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b);
     }
 }
 
@@ -80,3 +77,16 @@ void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
     }
 }
 
+void btBackwardEulerObjective::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] * 1;
+            ++counter;
+        }
+    }
+}
diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h
index 9b94341b7..e9642f6ae 100644
--- a/src/BulletSoftBody/btBackwardEulerObjective.h
+++ b/src/BulletSoftBody/btBackwardEulerObjective.h
@@ -76,15 +76,6 @@ class btBackwardEulerObjective
 {
 public:
     using TVStack = btAlignedObjectArray<btVector3>;
-//    struct DefaultPreconditioner
-//    {
-//        void operator()(const TVStack& x, TVStack& b)
-//        {
-//            btAssert(b.size() == x.size());
-//            for (int i = 0; i < b.size(); ++i)
-//                b[i] = x[i];
-//        }
-//    };
     btScalar m_dt;
     btConjugateGradient<btBackwardEulerObjective> cg;
     btDeformableRigidDynamicsWorld* m_world;
@@ -102,11 +93,29 @@ public:
     
     void computeResidual(btScalar dt, TVStack& residual) const
     {
-        // gravity is treated explicitly in predictUnconstraintMotion
-        // add force
+        // add implicit force
         for (int i = 0; i < m_lf.size(); ++i)
         {
-            m_lf[i]->addScaledForce(dt, residual);
+            m_lf[i]->addScaledImplicitForce(dt, residual);
+        }
+    }
+    
+    void applyExplicitForce(TVStack& force)
+    {
+        for (int i = 0; i < m_lf.size(); ++i)
+            m_lf[i]->addScaledExplicitForce(m_dt, force);
+        
+        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];
+                force[counter].setZero();
+                counter++;
+            }
         }
     }
     
@@ -117,7 +126,7 @@ public:
         {
             norm_squared += residual[i].length2();
         }
-        return std::sqrt(norm_squared);
+        return std::sqrt(norm_squared+SIMD_EPSILON);
     }
     
     void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
@@ -128,6 +137,7 @@ public:
     {
         projection.update(dv, m_backupVelocity);
     }
+    void initialGuess(TVStack& dv, const TVStack& residual);
     
     void reinitialize(bool nodeUpdated);
     
diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
index b0f798dad..a00fe384b 100644
--- a/src/BulletSoftBody/btCGProjection.h
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -40,13 +40,17 @@ struct Constraint
 struct Friction
 {
     btAlignedObjectArray<bool> m_static;
-    btAlignedObjectArray<btScalar> m_value;
+    btAlignedObjectArray<btScalar> m_impulse;
+    btAlignedObjectArray<btScalar> m_dv;
     btAlignedObjectArray<btVector3> m_direction;
     
     btAlignedObjectArray<bool> m_static_prev;
-    btAlignedObjectArray<btScalar> m_value_prev;
+    btAlignedObjectArray<btScalar> m_impulse_prev;
+    btAlignedObjectArray<btScalar> m_dv_prev;
     btAlignedObjectArray<btVector3> m_direction_prev;
     
+    btAlignedObjectArray<bool> m_released;
+    
     btAlignedObjectArray<btVector3> m_accumulated_impulse;
     Friction()
     {
@@ -56,10 +60,14 @@ struct Friction
         m_direction.push_back(btVector3(0,0,0));
         m_direction_prev.push_back(btVector3(0,0,0));
         
-        m_value.push_back(0);
-        m_value_prev.push_back(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_impulse.push_back(btVector3(0,0,0));
+        m_released.push_back(false);
     }
 };
 
diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp
index e356da264..3250e010b 100644
--- a/src/BulletSoftBody/btContactProjection.cpp
+++ b/src/BulletSoftBody/btContactProjection.cpp
@@ -85,35 +85,41 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                     // start friction handling
                     // copy old data
                     friction.m_direction_prev[j] = friction.m_direction[j];
-                    friction.m_value_prev[j] = friction.m_value[j];
+                    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];
                     if (accumulated_normal < 0 && accumulated_tangent.norm() > SIMD_EPSILON)
                     {
+                        
+                        btScalar compare1 = -accumulated_normal*c->m_c3;
+                        btScalar compare2 = accumulated_tangent.norm();
                         // do not allow switching from static friction to dynamic friction
                         // it causes cg to explode
-                        if (-accumulated_normal*c->m_c3 < accumulated_tangent.norm() && friction.m_static_prev[j] == false)
+                        if (-accumulated_normal*c->m_c3 < accumulated_tangent.norm() && friction.m_static_prev[j] == false && friction.m_released[j] == false)
                         {
                             friction.m_static[j] = false;
-                            friction.m_value[j] = -accumulated_normal*c->m_c3;
+                            friction.m_impulse[j] = -accumulated_normal*c->m_c3;
+                            friction.m_dv[j] = -accumulated_normal*c->m_c3 *c->m_c2/m_dt ;
                         }
                         else
                         {
                             friction.m_static[j] = true;
-                            friction.m_value[j] = accumulated_tangent.norm();
+                            friction.m_impulse[j] = accumulated_tangent.norm();
+                            friction.m_dv[j] = accumulated_tangent.norm() * c->m_c2/m_dt;
                         }
                         
                         const btVector3 tangent_dir = accumulated_tangent.normalized();
-                        impulse_tangent = friction.m_value[j] * tangent_dir;
                         friction.m_direction[j] = -tangent_dir;
                     }
                     else
                     {
+                        friction.m_released[j] = true;
                         friction.m_static[j] = false;
-                        friction.m_value[j] = 0;
-                        impulse_tangent.setZero();
+                        friction.m_impulse[j] = 0;
+                        friction.m_dv[j] = 0;
                     }
                     
-                    
+                    friction.m_accumulated_impulse[j] = accumulated_normal * cti.m_normal - friction.m_impulse[j] * friction.m_direction[j];
                     if (1) // in the same CG solve, the set of constraits doesn't change
 //                    if (dn <= SIMD_EPSILON)
                     {
@@ -128,8 +134,8 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                         // 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 + (friction.m_value_prev[j] * friction.m_direction_prev[j]) - (friction.m_value[j] * friction.m_direction[j]);
-                        
+                        impulse = impulse_normal + (friction.m_impulse_prev[j] * friction.m_direction_prev[j]) - (friction.m_impulse[j] * friction.m_direction[j]);
+//                        impulse = impulse_normal;
                         if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                         {
                             if (rigidCol)
@@ -172,7 +178,6 @@ void btContactProjection::setConstraintDirections()
                 f.push_back(Friction());
                 f.push_back(Friction());
                 m_frictions[&(psb->m_nodes[j])] = f;
-                // no friction constraints for dirichlet
             }
         }
     }
@@ -262,11 +267,14 @@ void btContactProjection::setConstraintDirections()
                                 // push in an empty friction
                                 frictions[j].m_direction.push_back(btVector3(0,0,0));
                                 frictions[j].m_direction_prev.push_back(btVector3(0,0,0));
-                                frictions[j].m_value.push_back(0);
-                                frictions[j].m_value_prev.push_back(0);
+                                frictions[j].m_impulse.push_back(0);
+                                frictions[j].m_impulse_prev.push_back(0);
+                                frictions[j].m_dv.push_back(0);
+                                frictions[j].m_dv_prev.push_back(0);
                                 frictions[j].m_static.push_back(false);
                                 frictions[j].m_static_prev.push_back(false);
                                 frictions[j].m_accumulated_impulse.push_back(btVector3(0,0,0));
+                                frictions[j].m_released.push_back(false);
                                 merged = true;
                                 break;
                             }
diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h
index b52bb2d72..969745d93 100644
--- a/src/BulletSoftBody/btContactProjection.h
+++ b/src/BulletSoftBody/btContactProjection.h
@@ -51,13 +51,13 @@ public:
                     // clear the old friction force
                     if (friction.m_static_prev[j] == false)
                     {
-                        x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j];
+                        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 && !has_static_constraint)
                     {
-                        x[i] += friction.m_direction[j] * friction.m_value[j];
+                        x[i] += friction.m_direction[j] * friction.m_impulse[j];
                     }
                 }
             }
@@ -68,6 +68,33 @@ public:
                 btAssert(free_dir.norm() > SIMD_EPSILON)
                 free_dir.normalize();
                 x[i] = x[i].dot(free_dir) * free_dir;
+                
+                bool has_static_constraint = false;
+                for (int f = 0; f < 2; ++f)
+                {
+                    Friction& 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 < 2; ++f)
+                {
+                    Friction& 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 && !has_static_constraint)
+                        {
+                            x[i] += friction.m_direction[j] * friction.m_impulse[j];
+                        }
+                    }
+                }
             }
             else
                 x[i].setZero();
@@ -97,12 +124,12 @@ public:
                     // clear the old constraint
                     if (friction.m_static_prev[j] == true)
                     {
-                        x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j];
+                        x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
                     }
                     // add the new constraint
                     if (friction.m_static[j] == true)
                     {
-                        x[i] += friction.m_direction[j] * friction.m_value[j];
+                        x[i] += friction.m_direction[j] * friction.m_dv[j];
                     }
                 }
             }
@@ -120,6 +147,24 @@ public:
                         x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
                     }
                 }
+                
+                for (int f = 0; f < 2; ++f)
+                {
+                    const Friction& friction= frictions[f];
+                    for (int j = 0; j < friction.m_direction.size(); ++j)
+                    {
+                        // clear the old constraint
+                        if (friction.m_static_prev[j] == true)
+                        {
+                            x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
+                        }
+                        // add the new constraint
+                        if (friction.m_static[j] == true)
+                        {
+                            x[i] += friction.m_direction[j] * friction.m_dv[j];
+                        }
+                    }
+                }
             }
             else
             {
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index e9bcf226a..a8e1e5cd5 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -118,7 +118,15 @@ void btDeformableBodySolver::solveConstraints(float solverdt)
     m_dt = solverdt;
     bool nodeUpdated = updateNodes();
     reinitialize(nodeUpdated);
+    
+    // apply explicit force
+    m_objective->applyExplicitForce(m_residual);
+    
+    // remove contact constraints with separating velocity
+    setConstraintDirections();
+    
     backupVelocity();
+    
     for (int i = 0; i < m_solveIterations; ++i)
     {
         m_objective->computeResidual(solverdt, m_residual);
@@ -143,9 +151,6 @@ void btDeformableBodySolver::reinitialize(bool nodeUpdated)
         m_residual[i].setZero();
     }
     m_objective->reinitialize(nodeUpdated);
-    
-    // remove contact constraints with separating velocity
-    setConstraintDirections();
 }
 
 void btDeformableBodySolver::setConstraintDirections()
diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h
index 3fac0743d..17f088c72 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.h
+++ b/src/BulletSoftBody/btDeformableBodySolver.h
@@ -74,20 +74,6 @@ public:
     
     void postStabilize();
     
-    void moveTempVelocity(btScalar dt, const TVStack& f)
-    {
-        size_t 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)
-            {
-                auto& node = psb->m_nodes[j];
-                node.m_v += node.m_im * dt * f[counter++];
-            }
-        }
-    }
-    
     void reinitialize(bool nodeUpdated);
     
     void setConstraintDirections();
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
index 5fdf01a18..173f71206 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
@@ -12,6 +12,7 @@
 
 void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
+    m_internalTime += timeStep;
     // Let the solver grab the soft bodies and if necessary optimize for it
     m_deformableBodySolver->optimize(m_softBodies);
     
@@ -54,7 +55,6 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
     // 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();
@@ -69,9 +69,14 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
     clearForces();
     clearMultiBodyForces();
     
+    
+    for (int i = 0; i < before_solver_callbacks.size(); ++i)
+        before_solver_callbacks[i](m_internalTime, this);
     ///solve deformable bodies constraints
     solveDeformableBodiesConstraints(timeStep);
 
+    
+    
     //integrate transforms
     btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
     
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
index ca1fff885..5bbebeda3 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
@@ -21,7 +21,7 @@
 #include "btMassSpring.h"
 #include "btDeformableBodySolver.h"
 #include "btSoftBodyHelpers.h"
-
+#include <functional>
 typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
 
 class btDeformableBodySolver;
@@ -40,6 +40,7 @@ class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld
     bool m_drawClusterTree;
     btSoftBodyWorldInfo m_sbi;
     bool m_ownsSolver;
+    btScalar m_internalTime;
     
 protected:
     virtual void internalSingleStepSimulation(btScalar timeStep);
@@ -67,8 +68,9 @@ public:
         m_sbi.m_gravity.setValue(0, -10, 0);
         
         m_sbi.m_sparsesdf.Initialize();
+        m_internalTime = 0.0;
     }
-    
+    btAlignedObjectArray<std::function<void(btScalar, btDeformableRigidDynamicsWorld*)> > before_solver_callbacks;
     virtual ~btDeformableRigidDynamicsWorld()
     {
     }
@@ -89,9 +91,6 @@ public:
     }
     
     virtual void predictUnconstraintMotion(btScalar timeStep);
-    // virtual void internalStepSingleStepSimulation(btScalar timeStep);
-    
-    // virtual void solveDeformableBodiesConstraints(btScalar timeStep);
     
     virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
     
diff --git a/src/BulletSoftBody/btLagrangianForce.h b/src/BulletSoftBody/btLagrangianForce.h
index 40e63207e..b95a30ce9 100644
--- a/src/BulletSoftBody/btLagrangianForce.h
+++ b/src/BulletSoftBody/btLagrangianForce.h
@@ -25,12 +25,14 @@ public:
     
     virtual ~btLagrangianForce(){}
     
-    virtual void addScaledForce(btScalar scale, TVStack& force) = 0;
+    virtual void addScaledImplicitForce(btScalar scale, TVStack& force) = 0;
     
     virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
     
     virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
     
+    virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
+    
     virtual void reinitialize(bool nodeUpdated)
     {
         if (nodeUpdated)
diff --git a/src/BulletSoftBody/btMassSpring.h b/src/BulletSoftBody/btMassSpring.h
index 9fd3c405a..78cdcf7e6 100644
--- a/src/BulletSoftBody/btMassSpring.h
+++ b/src/BulletSoftBody/btMassSpring.h
@@ -19,7 +19,42 @@ public:
         
     }
     
-    virtual void addScaledForce(btScalar scale, TVStack& force)
+    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 auto& link = psb->m_links[j];
+                const auto node1 = link.m_n[0];
+                const auto node2 = link.m_n[1];
+                size_t id1 = m_indices[node1];
+                size_t id2 = m_indices[node2];
+                
+                // 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())
@@ -31,7 +66,7 @@ public:
                 const auto& link = psb->m_links[j];
                 const auto node1 = link.m_n[0];
                 const auto node2 = link.m_n[1];
-                btScalar kLST = link.Feature::m_material->m_kLST; // this is probly wrong, TODO: figure out how to get stiffness
+                btScalar kLST = link.Feature::m_material->m_kLST;
                 btScalar r = link.m_rl;
                 size_t id1 = m_indices[node1];
                 size_t id2 = m_indices[node2];
@@ -39,21 +74,14 @@ public:
                 // elastic force
                 
                 // fully implicit
-                btVector3 dir = (node2->m_x - node1->m_x);
+//                btVector3 dir = (node2->m_x - node1->m_x);
                 
                 // explicit elastic force
-//                btVector3 dir = (node2->m_q - node1->m_q);
+                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;
-                
-                // damping force
-                btVector3 v_diff = (node2->m_v - node1->m_v);
-                btScalar k_damp = psb->m_dampingCoefficient; // TODO: FIX THIS HACK and set k_damp properly
-                scaled_force = scale * v_diff * k_damp;
-                force[id1] += scaled_force;
-                force[id2] -= scaled_force;
             }
         }
     }
@@ -64,7 +92,7 @@ public:
         btAssert(numNodes == dx.size());
         btAssert(numNodes == df.size());
         
-        // implicit elastic force
+        // implicit elastic force differential
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             const btSoftBody* psb = m_softBodies[i];
@@ -83,9 +111,10 @@ public:
         }
     }
     
+    
     virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
     {
-        // implicity damping force
+        // implicit damping force differential
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             const btSoftBody* psb = m_softBodies[i];
diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index 066072426..974246448 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -1782,7 +1782,7 @@ void btSoftBody::predictMotion(btScalar dt)
 	m_sst.radmrg = getCollisionShape()->getMargin();
 	m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
 	/* Forces				*/
-	addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
+    addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
 	applyForces();
 	/* Integrate			*/
 	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
@@ -1805,7 +1805,7 @@ void btSoftBody::predictMotion(btScalar dt)
 				}
 			}
 		}
-		n.m_v += deltaV;
+        n.m_v += deltaV;
 		n.m_x += n.m_v * m_sst.sdt;
 		n.m_f = btVector3(0, 0, 0);
 	}
@@ -2775,6 +2775,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)
 {
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index 01cde89e3..e8fd3da39 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/src/BulletSoftBody/btSoftBody.h
@@ -1011,6 +1011,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);
@@ -1021,7 +1022,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;