fix bugs in poststablize and projection of colinear constraints

5 files changed, 89 insertions, 69 deletions

diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp
index b90c749f3..6082f4dad 100644
--- a/src/BulletSoftBody/btContactProjection.cpp
+++ b/src/BulletSoftBody/btContactProjection.cpp
@@ -11,6 +11,7 @@
 void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity)
 {
     ///solve rigid body constraints
+    m_world->getSolverInfo().m_numIterations = 5;
     m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo());
 
     // loop through constraints to set constrained values
@@ -258,21 +259,21 @@ void btContactProjection::setConstraintDirections()
         }
         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);
-            }
+//            // 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 29d0e6632..8ce3cb578 100644
--- a/src/BulletSoftBody/btContactProjection.h
+++ b/src/BulletSoftBody/btContactProjection.h
@@ -47,8 +47,13 @@ public:
             {
                 // 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;
+                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;
+                }
             }
             else
                 x[i].setZero();
@@ -69,7 +74,8 @@ public:
             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;
+                btVector3  diff = constraints[0].m_value * constraints[0].m_direction;
+                x[i] += diff;
                 if (friction.m_direction.norm() > SIMD_EPSILON)
                 {
                     x[i] -= x[i].dot(friction.m_direction) * friction.m_direction;
@@ -79,8 +85,17 @@ public:
             else if (constraints.size() == 2)
             {
                 btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
-                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;
+                if (free_dir.norm() < SIMD_EPSILON)
+                {
+                    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;
+                }
+                else
+                {
+                    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;
+                }
             }
             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;
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index 41a5df70d..e9bcf226a 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -77,8 +77,8 @@ void btDeformableBodySolver::postStabilize()
                 const btVector3 vr = vb - va;
                 const btScalar dn = btDot(vr, cti.m_normal);
                 
-                const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
-                
+                btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
+//                dp += mrg;
                 // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
                 
                 btScalar dvn = dn * c.m_c4;
@@ -86,23 +86,27 @@ void btDeformableBodySolver::postStabilize()
                 // TODO: only contact is considered here, add friction later
                 if (dp < 0)
                 {
-                    c.m_node->m_x -= dp * cti.m_normal * c.m_c4;
-//                    c.m_node->m_x -= impulse * c.m_c2;
-                    
-                    ////
-                    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)
+                    bool two_way = false;
+                    if (two_way)
                     {
-                        if (multibodyLinkCol)
+                        c.m_node->m_x -= impulse * c.m_c2;
+                        
+                        if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                         {
-                            double multiplier = 0.5;
-                            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
+                            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
+                        c.m_node->m_x -= dp * cti.m_normal * c.m_c4;
                 }
             }
         }
@@ -115,7 +119,6 @@ void btDeformableBodySolver::solveConstraints(float solverdt)
     bool nodeUpdated = updateNodes();
     reinitialize(nodeUpdated);
     backupVelocity();
-    postStabilize();
     for (int i = 0; i < m_solveIterations; ++i)
     {
         m_objective->computeResidual(solverdt, m_residual);
@@ -123,7 +126,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt)
         updateVelocity();
     }
     advect(solverdt);
-//    postStabilize();
+    postStabilize();
 }
 
 void btDeformableBodySolver::reinitialize(bool nodeUpdated)
diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h
index 540a2b6cb..3fac0743d 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.h
+++ b/src/BulletSoftBody/btDeformableBodySolver.h
@@ -2,7 +2,7 @@
 //  btDeformableBodySolver.h
 //  BulletSoftBody
 //
-//  Created by Chuyuan Fu on 7/1/19.
+//  Created by Xuchen Han on 7/1/19.
 //
 
 #ifndef BT_DEFORMABLE_BODY_SOLVERS_H
@@ -101,8 +101,7 @@ public:
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
                 auto& node = psb->m_nodes[j];
-                node.m_x +=  dt * m_dv[counter++];
-//                node.m_x  =  node.m_q + dt * node.m_v;
+                node.m_x  =  node.m_q + dt * node.m_v;
             }
         }
     }
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index 12d96171f..b892498ea 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -869,35 +869,37 @@ struct btSoftColliders
 			const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
 			btSoftBody::RContact c;
 
-			if ((!n.m_battach) &&
-				psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
-			{
-				const btScalar ima = n.m_im;
-				const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
-				const btScalar ms = ima + imb;
-				if (ms > 0)
-				{
-					const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
-					static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
-					const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
-					const btVector3 ra = n.m_q - wtr.getOrigin();
-					const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
-					const btVector3 vb = n.m_x - n.m_q;
-					const btVector3 vr = vb - va;
-					const btScalar dn = btDot(vr, c.m_cti.m_normal);
-					const btVector3 fv = vr - c.m_cti.m_normal * dn;
-					const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
-					c.m_node = &n;
-					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 = 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)
-						m_rigidBody->activate();
-				}
+			if (!n.m_battach)
+            {
+				if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
+                {
+                    const btScalar ima = n.m_im;
+                    const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
+                    const btScalar ms = ima + imb;
+                    if (ms > 0)
+                    {
+                        const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
+                        static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+                        const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+                        const btVector3 ra = n.m_x - wtr.getOrigin();
+                        const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
+                        const btVector3 vb = n.m_x - n.m_q;
+                        const btVector3 vr = vb - va;
+                        const btScalar dn = btDot(vr, c.m_cti.m_normal);
+                        const btVector3 fv = vr - c.m_cti.m_normal * dn;
+                        const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
+                        c.m_node = &n;
+                        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 = 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)
+                            m_rigidBody->activate();
+                    }
+                }
 			}
 		}
 		btSoftBody* psb;