reformulate how constraints are managed in the projection class

5 files changed, 171 insertions, 136 deletions

diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp
index 69f5cd6ac..bdb781897 100644
--- a/src/BulletSoftBody/btBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp
@@ -70,19 +70,27 @@ void btBackwardEulerObjective::computeStep(TVStack& dv, const TVStack& residual,
 
 void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
 {
-    for (int i = 0; i < m_softBodies.size(); ++i)
+    // only the velocity of the constrained nodes needs to be updated during CG solve
+    for (auto it : projection.m_constraints)
     {
-        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_constrainedDirections.size() > 0)
-                    psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter];
-                ++counter;
-            }
-        }
+        int i = projection.m_indices[it.first];
+        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 ff1d6cbdd..a238e998b 100644
--- a/src/BulletSoftBody/btCGProjection.h
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -11,6 +11,34 @@
 #include <unordered_map>
 
 class btDeformableRigidDynamicsWorld;
+
+struct Constraint
+{
+    const btSoftBody::RContact* m_contact;
+    const btVector3 m_direction;
+    btScalar m_value;
+    
+    Constraint(const btSoftBody::RContact& rcontact)
+    : m_contact(&rcontact)
+    , m_direction(rcontact.m_cti.m_normal)
+    , m_value(0)
+    {
+    }
+    
+    Constraint(const btVector3 dir)
+    : m_contact(nullptr)
+    , m_direction(dir)
+    , m_value(0)
+    {}
+    
+    Constraint()
+    : m_contact(nullptr)
+    {
+        
+    }
+    
+};
+
 class btCGProjection
 {
 public:
@@ -21,11 +49,11 @@ 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;
-
+//    TVArrayStack m_constrainedDirections;
+//    TArrayStack m_constrainedValues;
+//    btAlignedObjectArray<int> m_constrainedId;
     const btScalar& m_dt;
+    std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<Constraint> > m_constraints;
     
     btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
     : m_softBodies(softBodies)
@@ -51,14 +79,15 @@ public:
             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_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();
     }
     
     void updateId()
diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp
index c45cccf5d..3016a9e85 100644
--- a/src/BulletSoftBody/btContactProjection.cpp
+++ b/src/BulletSoftBody/btContactProjection.cpp
@@ -12,20 +12,21 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
     ///solve rigid body constraints
     m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo());
 
-    // loop through contacts to create contact constraints
-    for (int i = 0; i < m_softBodies.size(); ++i)
+    // loop through constraints to set constrained values
+    for (auto it : m_constraints)
     {
-        btSoftBody* psb = m_softBodies[i];
-        btMultiBodyJacobianData jacobianData;
-        for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i)
+        btAlignedObjectArray<Constraint>& constraints = it.second;
+        for (int i = 0; i < constraints.size(); ++i)
         {
-            const btSoftBody::RContact& c = psb->m_rcontacts[i];
-            
-            // skip anchor points
-            if (c.m_node->m_im == 0)
+            Constraint& constraint = constraints[i];
+            if (constraint.m_contact == nullptr)
+            {
+                // nothing needs to be done for dirichelet constraints
                 continue;
-            
-            const btSoftBody::sCti& cti = c.m_cti;
+            }
+            const btSoftBody::RContact* c = constraint.m_contact;
+            const btSoftBody::sCti& cti = c->m_cti;
+            btMultiBodyJacobianData jacobianData;
             if (cti.m_colObj->hasContactResponse())
             {
                 btVector3 va(0, 0, 0);
@@ -37,7 +38,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                 {
                     rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
-                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0);
+                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0);
                 }
                 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                 {
@@ -49,7 +50,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                         jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
                         btScalar* jac = &jacobianData.m_jacobians[0];
                         
-                        multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
+                        multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c->m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
                         deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
                         multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
                         
@@ -62,26 +63,26 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                     }
                 }
                 
-                const btVector3 vb = c.m_node->m_v * m_dt;
+                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)
+                    //                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);
+                    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
                     // 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]];
+                    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);
-                    m_constrainedValues[m_indices[c.m_node]][0]=(dvn);
+                    constraint.m_value = dvn;
                     
                     if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                     {
                         if (rigidCol)
-                            rigidCol->applyImpulse(impulse, c.m_c1);
+                            rigidCol->applyImpulse(impulse, c->m_c1);
                     }
                     else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                     {
@@ -97,24 +98,23 @@ 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)
     {
-        const btSoftBody* psb = m_softBodies[i];
+        btSoftBody* psb = m_softBodies[i];
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
             if (psb->m_nodes[j].m_im == 0)
             {
-                m_constrainedDirections[counter].push_back(btVector3(1,0,0));
-                m_constrainedDirections[counter].push_back(btVector3(0,1,0));
-                m_constrainedDirections[counter].push_back(btVector3(0,0,1));
-                m_constrainedValues[counter].push_back(0);
-                m_constrainedValues[counter].push_back(0);
-                m_constrainedValues[counter].push_back(0);
-                m_constrainedId.push_back(counter);
+                btAlignedObjectArray<Constraint> c;
+                c.push_back(Constraint(btVector3(1,0,0)));
+                c.push_back(Constraint(btVector3(0,1,0)));
+                c.push_back(Constraint(btVector3(0,0,1)));
+                m_constraints[&(psb->m_nodes[j])] = c;
             }
             ++counter;
         }
@@ -125,14 +125,12 @@ void btContactProjection::setConstraintDirections()
         btSoftBody* psb = m_softBodies[i];
         btMultiBodyJacobianData jacobianData;
         
-        int j = 0;
-        while (j < psb->m_rcontacts.size())
+        for (int j = 0; j < psb->m_rcontacts.size(); ++j)
         {
             const btSoftBody::RContact& c = psb->m_rcontacts[j];
             // skip anchor points
             if (c.m_node->m_im == 0)
             {
-                psb->m_rcontacts.removeAtIndex(j);
                 continue;
             }
             
@@ -178,68 +176,69 @@ void btContactProjection::setConstraintDirections()
                 const btScalar dn = btDot(vr, cti.m_normal);
                 if (dn < SIMD_EPSILON)
                 {
-                    ++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]);
+                    if (m_constraints.find(c.m_node) == m_constraints.end())
+                    {
+                        btAlignedObjectArray<Constraint> constraints;
+                        constraints.push_back(Constraint(c));
+                        m_constraints[c.m_node] = constraints;
+                    }
+                    else
+                    {
+                        m_constraints[c.m_node].push_back(Constraint(c));
+                    }
                     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)
+    for (auto it : m_constraints)
     {
-        const btSoftBody* psb = m_softBodies[i];
-        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        const btAlignedObjectArray<Constraint>& c = it.second;
+        if (c.size() > dim)
         {
-            if (m_constrainedDirections[counter].size() > dim)
+            btAlignedObjectArray<Constraint> prunedConstraints;
+            // always keep the first constrained direction
+            prunedConstraints.push_back(c[0]);
+            
+            // find the direction most orthogonal to the first direction and keep it
+            size_t selected = 1;
+            btScalar min_dotProductAbs = std::abs(prunedConstraints[0].m_direction.dot(c[selected].m_direction));
+            for (int j = 2; j < c.size(); ++j)
             {
-                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].m_direction.dot(c[j].m_direction));
+                if (dotProductAbs < min_dotProductAbs)
                 {
-                    btScalar dotProductAbs =std::abs(prunedConstraints[0].dot(m_constrainedDirections[counter][j]));
-                    if (dotProductAbs < min_dotProductAbs)
-                    {
-                        selected = j;
-                        min_dotProductAbs = 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)
+            }
+            if (std::abs(min_dotProductAbs-1) < SIMD_EPSILON)
+            {
+                it.second = prunedConstraints;
+                continue;
+            }
+            prunedConstraints.push_back(c[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].m_direction, prunedConstraints[1].m_direction);
+            normal.normalize();
+            btScalar max_dotProductAbs = std::abs(normal.dot(c[selected2].m_direction));
+            for (int j = 3; j < c.size(); ++j)
+            {
+                btScalar dotProductAbs = std::abs(normal.dot(c[j].m_direction));
+                if (dotProductAbs > min_dotProductAbs)
                 {
-                    btScalar dotProductAbs = std::abs(normal.dot(m_constrainedDirections[counter][j]));
-                    if (dotProductAbs > min_dotProductAbs)
-                    {
-                        selected2 = j;
-                        max_dotProductAbs = dotProductAbs;
-                    }
+                    selected2 = j;
+                    max_dotProductAbs = dotProductAbs;
                 }
-                prunedConstraints.push_back(m_constrainedDirections[counter][selected2]);
-                m_constrainedDirections[counter] = prunedConstraints;
-                m_constrainedValues[counter].resize(dim);
             }
-            ++counter;
+            prunedConstraints.push_back(c[selected2]);
+            it.second = prunedConstraints;
         }
     }
 }
diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h
index c0897e9bf..a497c933c 100644
--- a/src/BulletSoftBody/btContactProjection.h
+++ b/src/BulletSoftBody/btContactProjection.h
@@ -30,20 +30,19 @@ public:
     virtual void operator()(TVStack& x)
     {
         const int dim = 3;
-        for (int j = 0; j < m_constrainedId.size(); ++j)
+        for (auto it : m_constraints)
         {
-            int i = m_constrainedId[j];
-            btAssert(m_constrainedDirections[i].size() <= dim);
-            if (m_constrainedDirections[i].size() <= 1)
+            const btAlignedObjectArray<Constraint>& constraints = it.second;
+            size_t i = m_indices[it.first];
+            btAssert(constraints.size() <= dim);
+            btAssert(constraints.size() > 0);
+            if (constraints.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] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
             }
-            else if (m_constrainedDirections[i].size() == 2)
+            else if (constraints.size() == 2)
             {
-                btVector3 free_dir = btCross(m_constrainedDirections[i][0], m_constrainedDirections[i][1]);
+                btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
                 free_dir.normalize();
                 x[i] = x[i].dot(free_dir) * free_dir;
             }
@@ -51,30 +50,30 @@ public:
                 x[i].setZero();
         }
     }
+
     
     virtual void enforceConstraint(TVStack& x)
     {
         const int dim = 3;
-        for (int j = 0; j < m_constrainedId.size(); ++j)
+        for (auto it : m_constraints)
         {
-            int i = m_constrainedId[j];
-            btAssert(m_constrainedDirections[i].size() <= dim);
-            if (m_constrainedDirections[i].size() <= 1)
+            const btAlignedObjectArray<Constraint>& constraints = it.second;
+            size_t i = m_indices[it.first];
+            btAssert(constraints.size() <= dim);
+            btAssert(constraints.size() > 0);
+            if (constraints.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];
-                }
+                x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
+                x[i] += constraints[0].m_value * constraints[0].m_direction;
             }
-            else if (m_constrainedDirections[i].size() == 2)
+            else if (constraints.size() == 2)
             {
-                btVector3 free_dir = btCross(m_constrainedDirections[i][0], m_constrainedDirections[i][1]);
+                btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
                 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];
+                x[i] = x[i].dot(free_dir) * free_dir + constraints[0].m_direction * constraints[0].m_value + constraints[1].m_direction * constraints[1].m_value;
             }
             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];
+                x[i] = constraints[0].m_value * constraints[0].m_direction + constraints[1].m_value * constraints[1].m_direction + constraints[2].m_value * constraints[2].m_direction;
         }
     }
     
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index 9f94e9f6b..b457a262f 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -89,20 +89,20 @@ void btDeformableBodySolver::postStabilize()
                     c.m_node->m_x -= dp * cti.m_normal * c.m_c4;
                     
                     ////
-                    //                        if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
-                    //                        {
-                    //                            if (rigidCol)
-                    //                                rigidCol->applyImpulse(impulse, c.m_c1);
-                    //                        }
+//                    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+//                    {
+//                        if (rigidCol)
+//                            rigidCol->applyImpulse(impulse, c.m_c1);
+//                    }
+//                    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+//                    {
+//                        if (multibodyLinkCol)
+//                        {
+//                            double multiplier = 0.5;
+//                            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
+//                        }
+//                    }
                 }
-                //                    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
-                //                    {
-                //                        if (multibodyLinkCol)
-                //                        {
-                //                            double multiplier = 0.5;
-                //                            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
-                //                        }
-                //                    }
             }
         }
     }