add support for more than one constraint for a single deformable node

4 files changed, 96 insertions, 9 deletions

diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp
index 22614c994..69f5cd6ac 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(20)
+: cg(50)
 , m_softBodies(softBodies)
 , precondition(DefaultPreconditioner())
 , projection(m_softBodies, m_dt)
@@ -66,7 +66,7 @@ void btBackwardEulerObjective::computeStep(TVStack& dv, const TVStack& residual,
     m_dt = dt;
     btScalar tolerance = std::numeric_limits<float>::epsilon()* 16 * computeNorm(residual);
     cg.solve(*this, dv, residual, tolerance);
-    }
+}
 
 void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
 {
diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
index 8648e99f8..ff1d6cbdd 100644
--- a/src/BulletSoftBody/btCGProjection.h
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -23,6 +23,8 @@ public:
     std::unordered_map<btSoftBody::Node *, size_t> m_indices;
     TVArrayStack m_constrainedDirections;
     TArrayStack m_constrainedValues;
+    btAlignedObjectArray<int> m_constrainedId;
+
     const btScalar& m_dt;
     
     btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
@@ -56,6 +58,7 @@ public:
             m_constrainedDirections[i].clear();
             m_constrainedValues[i].clear();
         }
+        m_constrainedId.clear();
     }
     
     void updateId()
diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp
index c7a947fbe..c45cccf5d 100644
--- a/src/BulletSoftBody/btContactProjection.cpp
+++ b/src/BulletSoftBody/btContactProjection.cpp
@@ -114,6 +114,7 @@ void btContactProjection::setConstraintDirections()
                 m_constrainedValues[counter].push_back(0);
                 m_constrainedValues[counter].push_back(0);
                 m_constrainedValues[counter].push_back(0);
+                m_constrainedId.push_back(counter);
             }
             ++counter;
         }
@@ -180,10 +181,65 @@ void btContactProjection::setConstraintDirections()
                     ++j;
                     m_constrainedDirections[m_indices[c.m_node]].push_back(cti.m_normal);
                     m_constrainedValues[m_indices[c.m_node]].resize(m_constrainedValues[m_indices[c.m_node]].size()+1);
+                    m_constrainedId.push_back(m_indices[c.m_node]);
                     continue;
                 }
             }
             psb->m_rcontacts.removeAtIndex(j);
         }
     }
+    
+    // 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 (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        const btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            if (m_constrainedDirections[counter].size() > dim)
+            {
+                btAlignedObjectArray<btVector3> prunedConstraints;
+                // always keep the first constrained direction
+                prunedConstraints.push_back(m_constrainedDirections[counter][0]);
+                // find the direction most orthogonal to the first direction and keep it
+                size_t selected = 1;
+                btScalar min_dotProductAbs = std::abs(prunedConstraints[0].dot(m_constrainedDirections[counter][selected]));
+                for (int j = 2; j < m_constrainedDirections[counter].size(); ++j)
+                {
+                    btScalar dotProductAbs =std::abs(prunedConstraints[0].dot(m_constrainedDirections[counter][j]));
+                    if (dotProductAbs < min_dotProductAbs)
+                    {
+                        selected = j;
+                        min_dotProductAbs = dotProductAbs;
+                    }
+                }
+                if (std::abs(min_dotProductAbs-1) < SIMD_EPSILON)
+                {
+                    m_constrainedDirections[counter++] = prunedConstraints;
+                    continue;
+                }
+                prunedConstraints.push_back(m_constrainedDirections[counter][selected]);
+                
+                // find the direction most orthogonal to the previous two directions and keep it
+                size_t selected2 = (selected == 1) ? 2 : 1;
+                btVector3 normal = btCross(prunedConstraints[0], prunedConstraints[1]);
+                normal.normalize();
+                btScalar max_dotProductAbs = std::abs(normal.dot(m_constrainedDirections[counter][selected2]));
+                for (int j = 3; j < m_constrainedDirections[counter].size(); ++j)
+                {
+                    btScalar dotProductAbs = std::abs(normal.dot(m_constrainedDirections[counter][j]));
+                    if (dotProductAbs > min_dotProductAbs)
+                    {
+                        selected2 = j;
+                        max_dotProductAbs = dotProductAbs;
+                    }
+                }
+                prunedConstraints.push_back(m_constrainedDirections[counter][selected2]);
+                m_constrainedDirections[counter] = prunedConstraints;
+                m_constrainedValues[counter].resize(dim);
+            }
+            ++counter;
+        }
+    }
 }
diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h
index 5598ada0a..c0897e9bf 100644
--- a/src/BulletSoftBody/btContactProjection.h
+++ b/src/BulletSoftBody/btContactProjection.h
@@ -29,24 +29,52 @@ public:
     // apply the constraints
     virtual void operator()(TVStack& x)
     {
-        for (int i = 0; i < x.size(); ++i)
+        const int dim = 3;
+        for (int j = 0; j < m_constrainedId.size(); ++j)
         {
-            for (int j = 0; j < m_constrainedDirections[i].size(); ++j)
+            int i = m_constrainedId[j];
+            btAssert(m_constrainedDirections[i].size() <= dim);
+            if (m_constrainedDirections[i].size() <= 1)
             {
-                x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j];
+                for (int j = 0; j < m_constrainedDirections[i].size(); ++j)
+                {
+                    x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j];
+                }
             }
+            else if (m_constrainedDirections[i].size() == 2)
+            {
+                btVector3 free_dir = btCross(m_constrainedDirections[i][0], m_constrainedDirections[i][1]);
+                free_dir.normalize();
+                x[i] = x[i].dot(free_dir) * free_dir;
+            }
+            else
+                x[i].setZero();
         }
     }
     
     virtual void enforceConstraint(TVStack& x)
     {
-        for (int i = 0; i < x.size(); ++i)
+        const int dim = 3;
+        for (int j = 0; j < m_constrainedId.size(); ++j)
         {
-            for (int j = 0; j < m_constrainedDirections[i].size(); ++j)
+            int i = m_constrainedId[j];
+            btAssert(m_constrainedDirections[i].size() <= dim);
+            if (m_constrainedDirections[i].size() <= 1)
+            {
+                for (int j = 0; j < m_constrainedDirections[i].size(); ++j)
+                {
+                    x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j];
+                    x[i] += m_constrainedValues[i][j] * m_constrainedDirections[i][j];
+                }
+            }
+            else if (m_constrainedDirections[i].size() == 2)
             {
-                x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j];
-                x[i] += m_constrainedValues[i][j] * m_constrainedDirections[i][j];
+                btVector3 free_dir = btCross(m_constrainedDirections[i][0], m_constrainedDirections[i][1]);
+                free_dir.normalize();
+                x[i] = x[i].dot(free_dir) * free_dir + m_constrainedDirections[i][0] * m_constrainedValues[i][0] + m_constrainedDirections[i][1] * m_constrainedValues[i][1];
             }
+            else
+                x[i] = m_constrainedDirections[i][0] * m_constrainedValues[i][0] + m_constrainedDirections[i][1] * m_constrainedValues[i][1] + m_constrainedDirections[i][2] * m_constrainedValues[i][2];
         }
     }