bug fix in friction; accumulate friction impulses in cg; forbid switching from static to dynamic friction

5 files changed, 121 insertions, 53 deletions

diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp
index 12c7a7add..e8c4c3edc 100644
--- a/src/BulletSoftBody/btBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp
@@ -8,7 +8,7 @@
 #include "btBackwardEulerObjective.h"
 
 btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
-: cg(50)
+: cg(20)
 , m_softBodies(softBodies)
 , projection(m_softBodies, m_dt)
 , m_backupVelocity(backup_v)
@@ -79,20 +79,4 @@ void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
         it.first->m_v = m_backupVelocity[i] + dv[i];
     }
 }
-    
-//    for (int i = 0; i < m_softBodies.size(); ++i)
-//    {
-//        int 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)
-//            {
-//                // only the velocity of the constrained nodes needs to be updated during CG solve
-//                if (projection.m_constraints[&(psb->m_nodes[j])].size() > 0)
-//                    psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter];
-//                ++counter;
-//            }
-//        }
-//    }
 
diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
index 91621e49b..b0f798dad 100644
--- a/src/BulletSoftBody/btCGProjection.h
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -39,17 +39,27 @@ struct Constraint
 
 struct Friction
 {
-    bool m_static;
-    btScalar m_value;
-    btVector3 m_direction;
+    btAlignedObjectArray<bool> m_static;
+    btAlignedObjectArray<btScalar> m_value;
+    btAlignedObjectArray<btVector3> m_direction;
     
-    bool m_static_prev;
-    btScalar m_value_prev;
-    btVector3 m_direction_prev;
+    btAlignedObjectArray<bool> m_static_prev;
+    btAlignedObjectArray<btScalar> m_value_prev;
+    btAlignedObjectArray<btVector3> m_direction_prev;
+    
+    btAlignedObjectArray<btVector3> m_accumulated_impulse;
     Friction()
     {
-        m_direction_prev.setZero();
-        m_direction.setZero();
+        m_static.push_back(false);
+        m_static_prev.push_back(false);
+        
+        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_accumulated_impulse.push_back(btVector3(0,0,0));
     }
 };
 
@@ -65,7 +75,7 @@ public:
     std::unordered_map<btSoftBody::Node *, size_t> m_indices;
     const btScalar& m_dt;
     std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<Constraint> > m_constraints;
-    std::unordered_map<btSoftBody::Node *, Friction > m_frictions;
+    std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<Friction> > m_frictions;
     
     btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
     : m_softBodies(softBodies)
diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h
index 0a88e6b94..a2b69f48d 100644
--- a/src/BulletSoftBody/btConjugateGradient.h
+++ b/src/BulletSoftBody/btConjugateGradient.h
@@ -63,11 +63,9 @@ public:
             //  r -= alpha * temp;
             multAndAddTo(alpha, p, x);
             multAndAddTo(-alpha, temp, r);
-            
             // zero out the dofs of r
             A.project(r,x);
             A.enforceConstraint(x);
-            
             r_norm = std::sqrt(squaredNorm(r));
             
             if (r_norm < tolerance) {
diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp
index 9388ac7e4..e356da264 100644
--- a/src/BulletSoftBody/btContactProjection.cpp
+++ b/src/BulletSoftBody/btContactProjection.cpp
@@ -17,11 +17,12 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
     // loop through constraints to set constrained values
     for (auto& it : m_constraints)
     {
-        Friction& friction = m_frictions[it.first];
+        btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
         btAlignedObjectArray<Constraint>& constraints = it.second;
         for (int i = 0; i < constraints.size(); ++i)
         {
             Constraint& constraint = constraints[i];
+            Friction& friction = frictions[i];
             for (int j = 0; j < constraint.m_contact.size(); ++j)
             {
                 if (constraint.m_contact[j] == nullptr)
@@ -75,36 +76,64 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                     const btVector3 impulse_normal = c->m_c0 *(cti.m_normal * dn);
                     btVector3 impulse_tangent = impulse - impulse_normal;
                     
-                    if (dn < 0 && impulse_tangent.norm() > SIMD_EPSILON)
+
+                    // accumulated impulse on the rb in this and all prev cg iterations
+                    friction.m_accumulated_impulse[j] += impulse;
+                    btScalar accumulated_normal = friction.m_accumulated_impulse[j].dot(cti.m_normal);
+                    btVector3 accumulated_tangent = friction.m_accumulated_impulse[j] - accumulated_normal * cti.m_normal;
+                    
+                    // 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_static_prev[j] = friction.m_static[j];
+                    if (accumulated_normal < 0 && accumulated_tangent.norm() > SIMD_EPSILON)
                     {
-                        btScalar impulse_tangent_magnitude = std::min(impulse_normal.norm()*c->m_c3*1000, impulse_tangent.norm());
-                        
-//                        impulse_tangent_magnitude = 0;
+                        // 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)
+                        {
+                            friction.m_static[j] = false;
+                            friction.m_value[j] = -accumulated_normal*c->m_c3;
+                        }
+                        else
+                        {
+                            friction.m_static[j] = true;
+                            friction.m_value[j] = accumulated_tangent.norm();
+                        }
                         
-                        const btVector3 tangent_dir = impulse_tangent.normalized();
-                        impulse_tangent = impulse_tangent_magnitude * tangent_dir;
-                        friction.m_direction = impulse_tangent;
-                        friction.m_dv = -impulse_tangent * c->m_c2/m_dt + (c->m_node->m_v - backupVelocity[m_indices[c->m_node]]);
+                        const btVector3 tangent_dir = accumulated_tangent.normalized();
+                        impulse_tangent = friction.m_value[j] * tangent_dir;
+                        friction.m_direction[j] = -tangent_dir;
+                    }
+                    else
+                    {
+                        friction.m_static[j] = false;
+                        friction.m_value[j] = 0;
+                        impulse_tangent.setZero();
                     }
-                    impulse = impulse_normal + impulse_tangent;
                     
-    //                if (1) // in the same CG solve, the set of constraits doesn't change
-                    if (dn <= SIMD_EPSILON)
+                    
+                    if (1) // in the same CG solve, the set of constraits doesn't change
+//                    if (dn <= SIMD_EPSILON)
                     {
                         // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
                         
-                        // TODO: only contact is considered here, add friction later
-                        
                         // dv = new_impulse + accumulated velocity change in previous CG iterations
                         // so we have the invariant node->m_v = backupVelocity + dv;
                         btVector3 dv = -impulse * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]];
                         btScalar dvn = dv.dot(cti.m_normal);
                         constraint.m_value[j] = dvn;
                         
+                        // the incremental impulse:
+                        // in the normal direction it's the normal component of "impulse"
+                        // in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration
+                        impulse = impulse_normal + (friction.m_value_prev[j] * friction.m_direction_prev[j]) - (friction.m_value[j] * friction.m_direction[j]);
+                        
                         if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                         {
                             if (rigidCol)
-                                rigidCol->applyImpulse(impulse_normal, c->m_c1);
+                                rigidCol->applyImpulse(impulse, c->m_c1);
                         }
                         else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                         {
@@ -137,6 +166,13 @@ void btContactProjection::setConstraintDirections()
                 c.push_back(Constraint(btVector3(0,1,0)));
                 c.push_back(Constraint(btVector3(0,0,1)));
                 m_constraints[&(psb->m_nodes[j])] = c;
+                
+                btAlignedObjectArray<Friction> f;
+                f.push_back(Friction());
+                f.push_back(Friction());
+                f.push_back(Friction());
+                m_frictions[&(psb->m_nodes[j])] = f;
+                // no friction constraints for dirichlet
             }
         }
     }
@@ -202,7 +238,9 @@ void btContactProjection::setConstraintDirections()
                         btAlignedObjectArray<Constraint> constraints;
                         constraints.push_back(Constraint(c));
                         m_constraints[c.m_node] = constraints;
-                        m_frictions[c.m_node] = Friction();
+                        btAlignedObjectArray<Friction> frictions;
+                        frictions.push_back(Friction());
+                        m_frictions[c.m_node] = frictions;
                     }
                     else
                     {
@@ -210,6 +248,7 @@ void btContactProjection::setConstraintDirections()
                         const btScalar angle_epsilon = 0.015192247; // less than 10 degree
                         bool merged = false;
                         btAlignedObjectArray<Constraint>& constraints = m_constraints[c.m_node];
+                        btAlignedObjectArray<Friction>& frictions = m_frictions[c.m_node];
                         for (int j = 0; j < constraints.size(); ++j)
                         {
                             const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction;
@@ -219,6 +258,15 @@ void btContactProjection::setConstraintDirections()
                                 constraints[j].m_contact.push_back(&c);
                                 constraints[j].m_direction.push_back(cti.m_normal);
                                 constraints[j].m_value.push_back(0);
+                                
+                                // 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_static.push_back(false);
+                                frictions[j].m_static_prev.push_back(false);
+                                frictions[j].m_accumulated_impulse.push_back(btVector3(0,0,0));
                                 merged = true;
                                 break;
                             }
@@ -226,7 +274,10 @@ void btContactProjection::setConstraintDirections()
                         const int dim = 3;
                         // hard coded no more than 3 constraint directions
                         if (!merged && constraints.size() < dim)
+                        {
                             constraints.push_back(Constraint(c));
+                            frictions.push_back(Friction());
+                        }
                     }
                 }
             }
diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h
index 1e5c52a60..b52bb2d72 100644
--- a/src/BulletSoftBody/btContactProjection.h
+++ b/src/BulletSoftBody/btContactProjection.h
@@ -34,16 +34,31 @@ public:
         {
             const btAlignedObjectArray<Constraint>& constraints = it.second;
             size_t i = m_indices[it.first];
-            const Friction& friction = m_frictions[it.first];
+            btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
             btAssert(constraints.size() <= dim);
             btAssert(constraints.size() > 0);
             if (constraints.size() == 1)
             {
                 x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
-                if (friction.m_direction.norm() > SIMD_EPSILON)
+                Friction& friction= frictions[0];
+                
+                bool has_static_constraint = false;
+                for (int j = 0; j < friction.m_static.size(); ++j)
+                    has_static_constraint = has_static_constraint || friction.m_static[j];
+                
+                for (int j = 0; j < friction.m_direction.size(); ++j)
                 {
-                    btVector3 dir = friction.m_direction.normalized();
-                    x[i] -= x[i].dot(dir) * dir;
+                    // clear the old friction force
+                    if (friction.m_static_prev[j] == false)
+                    {
+                        x[i] -= friction.m_direction_prev[j] * friction.m_value_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];
+                    }
                 }
             }
             else if (constraints.size() == 2)
@@ -67,7 +82,7 @@ public:
         {
             const btAlignedObjectArray<Constraint>& constraints = it.second;
             size_t i = m_indices[it.first];
-            const Friction& friction = m_frictions[it.first];
+            const btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
             btAssert(constraints.size() <= dim);
             btAssert(constraints.size() > 0);
             if (constraints.size() == 1)
@@ -75,15 +90,25 @@ public:
                 x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
                 for (int j = 0; j < constraints[0].m_direction.size(); ++j)
                     x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j];
-                if (friction.m_direction.norm() > SIMD_EPSILON)
+                
+                const Friction& friction= frictions[0];
+                for (int j = 0; j < friction.m_direction.size(); ++j)
                 {
-                    btVector3 dir = friction.m_direction.normalized();
-                    x[i] -= x[i].dot(dir) * dir;
-                    x[i] += friction.m_dv;
+                    // clear the old constraint
+                    if (friction.m_static_prev[j] == true)
+                    {
+                        x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j];
+                    }
+                    // add the new constraint
+                    if (friction.m_static[j] == true)
+                    {
+                        x[i] += friction.m_direction[j] * friction.m_value[j];
+                    }
                 }
             }
             else if (constraints.size() == 2)
             {
+                // TODO: friction
                 btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
                 btAssert(free_dir.norm() > SIMD_EPSILON)
                 free_dir.normalize();