reorganize the contact constraints

3 files changed, 148 insertions, 180 deletions

diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
index 6c104ef2f..91621e49b 100644
--- a/src/BulletSoftBody/btCGProjection.h
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -14,37 +14,41 @@ class btDeformableRigidDynamicsWorld;
 
 struct Constraint
 {
-    const btSoftBody::RContact* m_contact;
-    btVector3 m_direction;
-    btScalar m_value;
+    btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
+    btAlignedObjectArray<btVector3> m_direction;
+    btAlignedObjectArray<btScalar> m_value;
     
     Constraint(const btSoftBody::RContact& rcontact)
-    : m_contact(&rcontact)
-    , m_direction(rcontact.m_cti.m_normal)
-    , m_value(0)
     {
+        m_contact.push_back(&rcontact);
+        m_direction.push_back(rcontact.m_cti.m_normal);
+        m_value.push_back(0);
     }
     
     Constraint(const btVector3 dir)
-    : m_contact(nullptr)
-    , m_direction(dir)
-    , m_value(0)
-    {}
+    {
+        m_contact.push_back(nullptr);
+        m_direction.push_back(dir);
+        m_value.push_back(0);
+    }
     
     Constraint()
-    : m_contact(nullptr)
     {
-        
     }
 };
 
 struct Friction
 {
-    btVector3 m_dv;
+    bool m_static;
+    btScalar m_value;
     btVector3 m_direction;
+    
+    bool m_static_prev;
+    btScalar m_value_prev;
+    btVector3 m_direction_prev;
     Friction()
     {
-        m_dv.setZero();
+        m_direction_prev.setZero();
         m_direction.setZero();
     }
 };
diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp
index 6082f4dad..9388ac7e4 100644
--- a/src/BulletSoftBody/btContactProjection.cpp
+++ b/src/BulletSoftBody/btContactProjection.cpp
@@ -22,93 +22,97 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
         for (int i = 0; i < constraints.size(); ++i)
         {
             Constraint& constraint = constraints[i];
-            if (constraint.m_contact == nullptr)
+            for (int j = 0; j < constraint.m_contact.size(); ++j)
             {
-                // nothing needs to be done for dirichelet constraints
-                continue;
-            }
-            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);
-                btRigidBody* rigidCol = 0;
-                btMultiBodyLinkCollider* multibodyLinkCol = 0;
-                btScalar* deltaV;
-                
-                // grab the velocity of the rigid body
-                if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                if (constraint.m_contact[j] == nullptr)
                 {
-                    rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
-                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0);
+                    // nothing needs to be done for dirichelet constraints
+                    continue;
                 }
-                else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                const btSoftBody::RContact* c = constraint.m_contact[j];
+                const btSoftBody::sCti& cti = c->m_cti;
+                btMultiBodyJacobianData jacobianData;
+                if (cti.m_colObj->hasContactResponse())
                 {
-                    multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
-                    if (multibodyLinkCol)
+                    btVector3 va(0, 0, 0);
+                    btRigidBody* rigidCol = 0;
+                    btMultiBodyLinkCollider* multibodyLinkCol = 0;
+                    btScalar* deltaV;
+                    
+                    // grab the velocity of the rigid body
+                    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                     {
-                        const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-                        jacobianData.m_jacobians.resize(ndof);
-                        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);
-                        deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
-                        multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
-                        
-                        btScalar vel = 0.0;
-                        for (int j = 0; j < ndof; ++j)
+                        rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+                        va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0);
+                    }
+                    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                    {
+                        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                        if (multibodyLinkCol)
                         {
-                            vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
+                            const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                            jacobianData.m_jacobians.resize(ndof);
+                            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);
+                            deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
+                            multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
+                            
+                            btScalar vel = 0.0;
+                            for (int j = 0; j < ndof; ++j)
+                            {
+                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
+                            }
+                            va = cti.m_normal * vel * m_dt;
                         }
-                        va = cti.m_normal * vel * 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);
-                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 vb = c->m_node->m_v * m_dt;
+                    const btVector3 vr = vb - va;
+                    const btScalar dn = btDot(vr, cti.m_normal);
+                    btVector3 impulse = c->m_c0 * vr;
+                    const btVector3 impulse_normal = c->m_c0 *(cti.m_normal * dn);
+                    btVector3 impulse_tangent = impulse - impulse_normal;
                     
-                    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
-                    
-                    // 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 = dvn;
-                    
-                    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                    if (dn < 0 && impulse_tangent.norm() > SIMD_EPSILON)
                     {
-                        if (rigidCol)
-                            rigidCol->applyImpulse(impulse_normal, c->m_c1);
+                        btScalar impulse_tangent_magnitude = std::min(impulse_normal.norm()*c->m_c3*1000, 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]]);
                     }
-                    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                    impulse = impulse_normal + impulse_tangent;
+                    
+    //                if (1) // in the same CG solve, the set of constraits doesn't change
+                    if (dn <= SIMD_EPSILON)
                     {
-                        if (multibodyLinkCol)
+                        // 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;
+                        
+                        if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                        {
+                            if (rigidCol)
+                                rigidCol->applyImpulse(impulse_normal, c->m_c1);
+                        }
+                        else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                         {
-                            double multiplier = 0.5;
-                            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
+                            if (multibodyLinkCol)
+                            {
+                                double multiplier = 0.5;
+                                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
+                            }
                         }
                     }
                 }
@@ -202,78 +206,30 @@ void btContactProjection::setConstraintDirections()
                     }
                     else
                     {
-                        m_constraints[c.m_node].push_back(Constraint(c));
+                        // group colinear constraints into one
+                        const btScalar angle_epsilon = 0.015192247; // less than 10 degree
+                        bool merged = false;
+                        btAlignedObjectArray<Constraint>& constraints = m_constraints[c.m_node];
+                        for (int j = 0; j < constraints.size(); ++j)
+                        {
+                            const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction;
+                            btScalar dot_prod = dirs[0].dot(cti.m_normal);
+                            if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon)
+                            {
+                                constraints[j].m_contact.push_back(&c);
+                                constraints[j].m_direction.push_back(cti.m_normal);
+                                constraints[j].m_value.push_back(0);
+                                merged = true;
+                                break;
+                            }
+                        }
+                        const int dim = 3;
+                        // hard coded no more than 3 constraint directions
+                        if (!merged && constraints.size() < dim)
+                            constraints.push_back(Constraint(c));
                     }
-                    continue;
                 }
             }
         }
     }
-    
-    // for particles with more than three constrained directions, prune constrained directions so that there are at most three constrained directions
-    const int dim = 3;
-    for (auto& it : m_constraints)
-    {
-        btAlignedObjectArray<Constraint>& c = it.second;
-        if (c.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)
-            {
-                btScalar dotProductAbs =std::abs(prunedConstraints[0].m_direction.dot(c[j].m_direction));
-                if (dotProductAbs < min_dotProductAbs)
-                {
-                    selected = j;
-                    min_dotProductAbs = dotProductAbs;
-                }
-            }
-            if (std::abs(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)
-                {
-                    selected2 = j;
-                    max_dotProductAbs = dotProductAbs;
-                }
-            }
-            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 8ce3cb578..1e5c52a60 100644
--- a/src/BulletSoftBody/btContactProjection.h
+++ b/src/BulletSoftBody/btContactProjection.h
@@ -39,21 +39,20 @@ public:
             btAssert(constraints.size() > 0);
             if (constraints.size() == 1)
             {
-                x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
+                x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
                 if (friction.m_direction.norm() > SIMD_EPSILON)
-                    x[i] -= x[i].dot(friction.m_direction) * friction.m_direction;
+                {
+                    btVector3 dir = friction.m_direction.normalized();
+                    x[i] -= x[i].dot(dir) * dir;
+                }
             }
             else if (constraints.size() == 2)
             {
                 // TODO : friction
-                btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
-                if (free_dir.norm() < SIMD_EPSILON)
-                    x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
-                else
-                {
-                    free_dir.normalize();
-                    x[i] = x[i].dot(free_dir) * free_dir;
-                }
+                btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
+                btAssert(free_dir.norm() > SIMD_EPSILON)
+                free_dir.normalize();
+                x[i] = x[i].dot(free_dir) * free_dir;
             }
             else
                 x[i].setZero();
@@ -73,32 +72,41 @@ public:
             btAssert(constraints.size() > 0);
             if (constraints.size() == 1)
             {
-                x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
-                btVector3  diff = constraints[0].m_value * constraints[0].m_direction;
-                x[i] += diff;
+                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)
                 {
-                    x[i] -= x[i].dot(friction.m_direction) * friction.m_direction;
+                    btVector3 dir = friction.m_direction.normalized();
+                    x[i] -= x[i].dot(dir) * dir;
                     x[i] += friction.m_dv;
                 }
             }
             else if (constraints.size() == 2)
             {
-                btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
-                if (free_dir.norm() < SIMD_EPSILON)
+                btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
+                btAssert(free_dir.norm() > SIMD_EPSILON)
+                free_dir.normalize();
+                x[i] = x[i].dot(free_dir) * free_dir;
+                for (int j = 0; j < constraints.size(); ++j)
                 {
-                    x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
-                    btVector3 diff = constraints[0].m_value * constraints[0].m_direction + constraints[1].m_value * constraints[1].m_direction;
-                    x[i] += diff;
+                    for (int k = 0; k < constraints[j].m_direction.size(); ++k)
+                    {
+                        x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
+                    }
                 }
-                else
+            }
+            else
+            {
+                x[i].setZero();
+                for (int j = 0; j < constraints.size(); ++j)
                 {
-                    free_dir.normalize();
-                    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;
+                    for (int k = 0; k < constraints[j].m_direction.size(); ++k)
+                    {
+                        x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
+                    }
                 }
             }
-            else
-                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;
         }
     }