bug fix in projection; start friction

6 files changed, 75 insertions, 30 deletions

diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp
index bdb781897..8801e68df 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)
 , precondition(DefaultPreconditioner())
 , projection(m_softBodies, m_dt)
diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
index a238e998b..6c104ef2f 100644
--- a/src/BulletSoftBody/btCGProjection.h
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -15,7 +15,7 @@ class btDeformableRigidDynamicsWorld;
 struct Constraint
 {
     const btSoftBody::RContact* m_contact;
-    const btVector3 m_direction;
+    btVector3 m_direction;
     btScalar m_value;
     
     Constraint(const btSoftBody::RContact& rcontact)
@@ -36,7 +36,17 @@ struct Constraint
     {
         
     }
-    
+};
+
+struct Friction
+{
+    btVector3 m_dv;
+    btVector3 m_direction;
+    Friction()
+    {
+        m_dv.setZero();
+        m_direction.setZero();
+    }
 };
 
 class btCGProjection
@@ -49,11 +59,9 @@ public:
     btAlignedObjectArray<btSoftBody *> m_softBodies;
     btDeformableRigidDynamicsWorld* m_world;
     std::unordered_map<btSoftBody::Node *, size_t> m_indices;
-//    TVArrayStack m_constrainedDirections;
-//    TArrayStack m_constrainedValues;
-//    btAlignedObjectArray<int> m_constrainedId;
     const btScalar& m_dt;
     std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<Constraint> > m_constraints;
+    std::unordered_map<btSoftBody::Node *, Friction > m_frictions;
     
     btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
     : m_softBodies(softBodies)
@@ -77,17 +85,8 @@ public:
     {
         if (nodeUpdated)
             updateId();
-        
-        // resize and clear the old constraints
-//        m_constrainedValues.resize(m_indices.size());
-//        m_constrainedDirections.resize(m_indices.size());
-//        for (int i = 0; i < m_constrainedDirections.size(); ++i)
-//        {
-//            m_constrainedDirections[i].clear();
-//            m_constrainedValues[i].clear();
-//        }
-//        m_constrainedId.clear();
         m_constraints.clear();
+        m_frictions.clear();
     }
     
     void updateId()
diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp
index 3016a9e85..b90c749f3 100644
--- a/src/BulletSoftBody/btContactProjection.cpp
+++ b/src/BulletSoftBody/btContactProjection.cpp
@@ -7,14 +7,16 @@
 
 #include "btContactProjection.h"
 #include "btDeformableRigidDynamicsWorld.h"
+#include <algorithm>
 void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity)
 {
     ///solve rigid body constraints
     m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo());
 
     // loop through constraints to set constrained values
-    for (auto it : m_constraints)
+    for (auto& it : m_constraints)
     {
+        Friction& friction = m_frictions[it.first];
         btAlignedObjectArray<Constraint>& constraints = it.second;
         for (int i = 0; i < constraints.size(); ++i)
         {
@@ -66,11 +68,27 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                 const btVector3 vb = c->m_node->m_v * m_dt;
                 const btVector3 vr = vb - va;
                 const btScalar dn = btDot(vr, cti.m_normal);
-                if (1) // in the same CG solve, the set of constraits doesn't change
-                    //                if (dn <= SIMD_EPSILON)
+                btVector3 impulse = c->m_c0 * vr;
+                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)
+                {
+                    btScalar impulse_tangent_magnitude = std::min(impulse_normal.norm()*c->m_c3, impulse_tangent.norm());
+                    
+                    impulse_tangent_magnitude = 0;
+                    
+                    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]]).dot(tangent_dir)*tangent_dir;
+                }
+                impulse = impulse_normal + impulse_tangent;
+//                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
-                    const btVector3 impulse = c->m_c0 *(cti.m_normal * dn);
+                    
                     // TODO: only contact is considered here, add friction later
                     
                     // dv = new_impulse + accumulated velocity change in previous CG iterations
@@ -82,7 +100,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                     if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                     {
                         if (rigidCol)
-                            rigidCol->applyImpulse(impulse, c->m_c1);
+                            rigidCol->applyImpulse(impulse_normal, c->m_c1);
                     }
                     else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                     {
@@ -102,7 +120,6 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
 void btContactProjection::setConstraintDirections()
 {
     // set Dirichlet constraint
-    size_t counter = 0;
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody* psb = m_softBodies[i];
@@ -116,7 +133,6 @@ void btContactProjection::setConstraintDirections()
                 c.push_back(Constraint(btVector3(0,0,1)));
                 m_constraints[&(psb->m_nodes[j])] = c;
             }
-            ++counter;
         }
     }
 
@@ -181,6 +197,7 @@ void btContactProjection::setConstraintDirections()
                         btAlignedObjectArray<Constraint> constraints;
                         constraints.push_back(Constraint(c));
                         m_constraints[c.m_node] = constraints;
+                        m_frictions[c.m_node] = Friction();
                     }
                     else
                     {
@@ -193,11 +210,10 @@ void btContactProjection::setConstraintDirections()
     }
     
     // for particles with more than three constrained directions, prune constrained directions so that there are at most three constrained directions
-    counter = 0;
     const int dim = 3;
-    for (auto it : m_constraints)
+    for (auto& it : m_constraints)
     {
-        const btAlignedObjectArray<Constraint>& c = it.second;
+        btAlignedObjectArray<Constraint>& c = it.second;
         if (c.size() > dim)
         {
             btAlignedObjectArray<Constraint> prunedConstraints;
@@ -216,7 +232,7 @@ void btContactProjection::setConstraintDirections()
                     min_dotProductAbs = dotProductAbs;
                 }
             }
-            if (std::abs(min_dotProductAbs-1) < SIMD_EPSILON)
+            if (std::abs(std::abs(min_dotProductAbs)-1) < SIMD_EPSILON)
             {
                 it.second = prunedConstraints;
                 continue;
@@ -240,5 +256,23 @@ void btContactProjection::setConstraintDirections()
             prunedConstraints.push_back(c[selected2]);
             it.second = prunedConstraints;
         }
+        else
+        {
+            // prune out collinear constraints
+            const btVector3& first_dir = c[0].m_direction;
+            int i = 1;
+            while (i < c.size())
+            {
+                if (std::abs(std::abs(first_dir.dot(c[i].m_direction)) - 1) < 4*SIMD_EPSILON)
+                    c.removeAtIndex(i);
+                else
+                    ++i;
+            }
+            if (c.size() == 3)
+            {
+                if (std::abs(std::abs(c[1].m_direction.dot(c[2].m_direction)) - 1) < 4*SIMD_EPSILON)
+                    c.removeAtIndex(2);
+            }
+        }
     }
 }
diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h
index a497c933c..29d0e6632 100644
--- a/src/BulletSoftBody/btContactProjection.h
+++ b/src/BulletSoftBody/btContactProjection.h
@@ -30,18 +30,22 @@ public:
     virtual void operator()(TVStack& x)
     {
         const int dim = 3;
-        for (auto it : m_constraints)
+        for (auto& it : m_constraints)
         {
             const btAlignedObjectArray<Constraint>& constraints = it.second;
             size_t i = m_indices[it.first];
+            const Friction& friction = 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) * constraints[0].m_direction;
+                if (friction.m_direction.norm() > SIMD_EPSILON)
+                    x[i] -= x[i].dot(friction.m_direction) * friction.m_direction;
             }
             else if (constraints.size() == 2)
             {
+                // TODO : friction
                 btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
                 free_dir.normalize();
                 x[i] = x[i].dot(free_dir) * free_dir;
@@ -55,16 +59,22 @@ public:
     virtual void enforceConstraint(TVStack& x)
     {
         const int dim = 3;
-        for (auto it : m_constraints)
+        for (auto& it : m_constraints)
         {
             const btAlignedObjectArray<Constraint>& constraints = it.second;
             size_t i = m_indices[it.first];
+            const Friction& friction = 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) * constraints[0].m_direction;
                 x[i] += constraints[0].m_value * constraints[0].m_direction;
+                if (friction.m_direction.norm() > SIMD_EPSILON)
+                {
+                    x[i] -= x[i].dot(friction.m_direction) * friction.m_direction;
+                    x[i] += friction.m_dv;
+                }
             }
             else if (constraints.size() == 2)
             {
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index b457a262f..b7c531385 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -122,6 +122,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt)
         updateVelocity();
     }
     advect(solverdt);
+//    postStabilize();
 }
 
 void btDeformableBodySolver::reinitialize(bool nodeUpdated)
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index 41911b2bc..12d96171f 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -891,7 +891,8 @@ struct btSoftColliders
 					c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
 					c.m_c1 = ra;
 					c.m_c2 = ima * psb->m_sst.sdt;
-					c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
+//                    c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
+                    c.m_c3 = fc;
 					c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
 					psb->m_rcontacts.push_back(c);
 					if (m_rigidBody)