bullet: Sync with upstream 3.07

1 files changed, 383 insertions, 378 deletions

diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp
index 22ca8bf582..7f67260ce6 100644
--- a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp
+++ b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp
@@ -17,7 +17,7 @@
 #include "btDeformableMultiBodyDynamicsWorld.h"
 #include <algorithm>
 #include <cmath>
-btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal)
+btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 {
 	btScalar residualSquare = 0;
 	for (int i = 0; i < numDeformableBodies; ++i)
@@ -58,27 +58,37 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod
 	return residualSquare;
 }
 
-void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
+btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 {
-	for (int i = 0; i < m_softBodies.size(); ++i)
+	btScalar residualSquare = 0;
+	for (int i = 0; i < numDeformableBodies; ++i)
 	{
-		// node constraints
-		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
-		{
-			btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
-			constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
-		}
-		// face constraints
-		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+		for (int j = 0; j < m_softBodies.size(); ++j)
 		{
-			btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
-			constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
+			btCollisionObject* psb = m_softBodies[j];
+			if (psb != deformableBodies[i])
+			{
+				continue;
+			}
+			for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
+			{
+				btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
+				residualSquare = btMax(residualSquare, localResidualSquare);
+			}
+			for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
+			{
+				btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
+				residualSquare = btMax(residualSquare, localResidualSquare);
+			}
 		}
 	}
+	return residualSquare;
 }
 
 void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
-{  
+{
 	BT_PROFILE("setConstraints");
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
@@ -97,7 +107,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
 				m_staticConstraints[i].push_back(static_constraint);
 			}
 		}
-		
+
 		// set up deformable anchors
 		for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
 		{
@@ -111,7 +121,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
 			btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal);
 			m_nodeAnchorConstraints[i].push_back(constraint);
 		}
-		
+
 		// set Deformable Node vs. Rigid constraint
 		for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
 		{
@@ -122,17 +132,9 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
 				continue;
 			}
 			btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
-			btVector3 va = constraint.getVa();
-			btVector3 vb = constraint.getVb();
-			const btVector3 vr = vb - va;
-			const btSoftBody::sCti& cti = contact.m_cti;
-			const btScalar dn = btDot(vr, cti.m_normal);
-			if (dn < SIMD_EPSILON)
-			{
-				m_nodeRigidConstraints[i].push_back(constraint);
-			}
+			m_nodeRigidConstraints[i].push_back(constraint);
 		}
-		
+
 		// set Deformable Face vs. Rigid constraint
 		for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
 		{
@@ -143,15 +145,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
 				continue;
 			}
 			btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
-			btVector3 va = constraint.getVa();
-			btVector3 vb = constraint.getVb();
-			const btVector3 vr = vb - va;
-			const btSoftBody::sCti& cti = contact.m_cti;
-			const btScalar dn = btDot(vr, cti.m_normal);
-			if (dn < SIMD_EPSILON)
-			{
-				m_faceRigidConstraints[i].push_back(constraint);
-			}
+			m_faceRigidConstraints[i].push_back(constraint);
 		}
 	}
 }
@@ -159,267 +153,269 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
 void btDeformableContactProjection::project(TVStack& x)
 {
 #ifndef USE_MGS
-    const int dim = 3;
-    for (int index = 0; index < m_projectionsDict.size(); ++index)
-    {
-        btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
-        size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
-        if (projectionDirs.size() >= dim)
-        {
-            // static node
-            x[i].setZero();
-            continue;
-        }
-        else if (projectionDirs.size() == 2)
-        {
-            btVector3 dir0 = projectionDirs[0];
-            btVector3 dir1 = projectionDirs[1];
-            btVector3 free_dir = btCross(dir0, dir1);
-            if (free_dir.safeNorm() < SIMD_EPSILON)
-            {
-                x[i] -= x[i].dot(dir0) * dir0;
-                x[i] -= x[i].dot(dir1) * dir1;
-            }
-            else
-            {
-                free_dir.normalize();
-                x[i] = x[i].dot(free_dir) * free_dir;
-            }
-        }
-        else
-        {
-            btAssert(projectionDirs.size() == 1);
-            btVector3 dir0 = projectionDirs[0];
-            x[i] -= x[i].dot(dir0) * dir0;
-        }
-    }
+	const int dim = 3;
+	for (int index = 0; index < m_projectionsDict.size(); ++index)
+	{
+		btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
+		size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
+		if (projectionDirs.size() >= dim)
+		{
+			// static node
+			x[i].setZero();
+			continue;
+		}
+		else if (projectionDirs.size() == 2)
+		{
+			btVector3 dir0 = projectionDirs[0];
+			btVector3 dir1 = projectionDirs[1];
+			btVector3 free_dir = btCross(dir0, dir1);
+			if (free_dir.safeNorm() < SIMD_EPSILON)
+			{
+				x[i] -= x[i].dot(dir0) * dir0;
+			}
+			else
+			{
+				free_dir.normalize();
+				x[i] = x[i].dot(free_dir) * free_dir;
+			}
+		}
+		else
+		{
+			btAssert(projectionDirs.size() == 1);
+			btVector3 dir0 = projectionDirs[0];
+			x[i] -= x[i].dot(dir0) * dir0;
+		}
+	}
 #else
-    btReducedVector p(x.size());
-    for (int i = 0; i < m_projections.size(); ++i)
-    {
-        p += (m_projections[i].dot(x) * m_projections[i]);
-    }
-    for (int i = 0; i < p.m_indices.size(); ++i)
-    {
-        x[p.m_indices[i]] -= p.m_vecs[i];
-    }
+	btReducedVector p(x.size());
+	for (int i = 0; i < m_projections.size(); ++i)
+	{
+		p += (m_projections[i].dot(x) * m_projections[i]);
+	}
+	for (int i = 0; i < p.m_indices.size(); ++i)
+	{
+		x[p.m_indices[i]] -= p.m_vecs[i];
+	}
 #endif
 }
 
 void btDeformableContactProjection::setProjection()
 {
 #ifndef USE_MGS
-    BT_PROFILE("btDeformableContactProjection::setProjection");
-    btAlignedObjectArray<btVector3> units;
-    units.push_back(btVector3(1,0,0));
-    units.push_back(btVector3(0,1,0));
-    units.push_back(btVector3(0,0,1));
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        if (!psb->isActive())
-        {
-            continue;
-        }
-        for (int j = 0; j < m_staticConstraints[i].size(); ++j)
-        {
-            int index = m_staticConstraints[i][j].m_node->index;
-            m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
-            if (m_projectionsDict.find(index) == NULL)
-            {
-                m_projectionsDict.insert(index, units);
-            }
-            else
-            {
-                btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-                for (int k = 0; k < 3; ++k)
-                {
-                    projections.push_back(units[k]);
-                }
-            }
-        }
-        for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
-        {
-            int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
-            m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
-            if (m_projectionsDict.find(index) == NULL)
-            {
-                m_projectionsDict.insert(index, units);
-            }
-            else
-            {
-                btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-                for (int k = 0; k < 3; ++k)
-                {
-                    projections.push_back(units[k]);
-                }
-            }
-        }
-        for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
-        {
-            int index = m_nodeRigidConstraints[i][j].m_node->index;
-            m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
-            if (m_nodeRigidConstraints[i][j].m_static)
-            {
-                if (m_projectionsDict.find(index) == NULL)
-                {
-                    m_projectionsDict.insert(index, units);
-                }
-                else
-                {
-                    btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-                    for (int k = 0; k < 3; ++k)
-                    {
-                        projections.push_back(units[k]);
-                    }
-                }
-            }
-            else
-            {
-                if (m_projectionsDict.find(index) == NULL)
-                {
-                    btAlignedObjectArray<btVector3> projections;
-                    projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
-                    m_projectionsDict.insert(index, projections);
-                }
-                else
-                {
-                    btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-                    projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
-                }
-            }
-        }
-        for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
-        {
-            const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
-            btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
-            for (int k = 0; k < 3; ++k)
-            {
-                face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
-            }
-            for (int k = 0; k < 3; ++k)
-            {
-                btSoftBody::Node* node = face->m_n[k];
-                node->m_penetration = true;
-                int index = node->index;
-                if (m_faceRigidConstraints[i][j].m_static)
-                {
-                    if (m_projectionsDict.find(index) == NULL)
-                    {
-                        m_projectionsDict.insert(index, units);
-                    }
-                    else
-                    {
-                        btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-                        for (int k = 0; k < 3; ++k)
-                        {
-                            projections.push_back(units[k]);
-                        }
-                    }
-                }
-                else
-                {
-                    if (m_projectionsDict.find(index) == NULL)
-                    {
-                        btAlignedObjectArray<btVector3> projections;
-                        projections.push_back(m_faceRigidConstraints[i][j].m_normal);
-                        m_projectionsDict.insert(index, projections);
-                    }
-                    else
-                    {
-                        btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-                        projections.push_back(m_faceRigidConstraints[i][j].m_normal);
-                    }
-                }
-            }
-        }
-    }
+	BT_PROFILE("btDeformableContactProjection::setProjection");
+	btAlignedObjectArray<btVector3> units;
+	units.push_back(btVector3(1, 0, 0));
+	units.push_back(btVector3(0, 1, 0));
+	units.push_back(btVector3(0, 0, 1));
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			continue;
+		}
+		for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+		{
+			int index = m_staticConstraints[i][j].m_node->index;
+			m_staticConstraints[i][j].m_node->m_constrained = true;
+			if (m_projectionsDict.find(index) == NULL)
+			{
+				m_projectionsDict.insert(index, units);
+			}
+			else
+			{
+				btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+				for (int k = 0; k < 3; ++k)
+				{
+					projections.push_back(units[k]);
+				}
+			}
+		}
+		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
+		{
+			int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
+			m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
+			if (m_projectionsDict.find(index) == NULL)
+			{
+				m_projectionsDict.insert(index, units);
+			}
+			else
+			{
+				btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+				for (int k = 0; k < 3; ++k)
+				{
+					projections.push_back(units[k]);
+				}
+			}
+		}
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+		{
+			int index = m_nodeRigidConstraints[i][j].m_node->index;
+			m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
+			if (m_nodeRigidConstraints[i][j].m_binding)
+			{
+				if (m_nodeRigidConstraints[i][j].m_static)
+				{
+					if (m_projectionsDict.find(index) == NULL)
+					{
+						m_projectionsDict.insert(index, units);
+					}
+					else
+					{
+						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+						for (int k = 0; k < 3; ++k)
+						{
+							projections.push_back(units[k]);
+						}
+					}
+				}
+				else
+				{
+					if (m_projectionsDict.find(index) == NULL)
+					{
+						btAlignedObjectArray<btVector3> projections;
+						projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
+						m_projectionsDict.insert(index, projections);
+					}
+					else
+					{
+						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+						projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
+					}
+				}
+			}
+		}
+		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+		{
+			const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
+			if (m_faceRigidConstraints[i][j].m_binding)
+			{
+				for (int k = 0; k < 3; ++k)
+				{
+					face->m_n[k]->m_constrained = true;
+				}
+			}
+			for (int k = 0; k < 3; ++k)
+			{
+				btSoftBody::Node* node = face->m_n[k];
+				int index = node->index;
+				if (m_faceRigidConstraints[i][j].m_static)
+				{
+					if (m_projectionsDict.find(index) == NULL)
+					{
+						m_projectionsDict.insert(index, units);
+					}
+					else
+					{
+						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+						for (int l = 0; l < 3; ++l)
+						{
+							projections.push_back(units[l]);
+						}
+					}
+				}
+				else
+				{
+					if (m_projectionsDict.find(index) == NULL)
+					{
+						btAlignedObjectArray<btVector3> projections;
+						projections.push_back(m_faceRigidConstraints[i][j].m_normal);
+						m_projectionsDict.insert(index, projections);
+					}
+					else
+					{
+						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+						projections.push_back(m_faceRigidConstraints[i][j].m_normal);
+					}
+				}
+			}
+		}
+	}
 #else
-    int dof = 0;
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        dof += m_softBodies[i]->m_nodes.size();
-    }
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        if (!psb->isActive())
-        {
-            continue;
-        }
-        for (int j = 0; j < m_staticConstraints[i].size(); ++j)
-        {
-            int index = m_staticConstraints[i][j].m_node->index;
-            m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
-            btAlignedObjectArray<int> indices;
-            btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3;
-            indices.push_back(index);
-            vecs1.push_back(btVector3(1,0,0));
-            vecs2.push_back(btVector3(0,1,0));
-            vecs3.push_back(btVector3(0,0,1));
-            m_projections.push_back(btReducedVector(dof, indices, vecs1));
-            m_projections.push_back(btReducedVector(dof, indices, vecs2));
-            m_projections.push_back(btReducedVector(dof, indices, vecs3));
-        }
-        
-        for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
-        {
-            int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
-            m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
-            btAlignedObjectArray<int> indices;
-            btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3;
-            indices.push_back(index);
-            vecs1.push_back(btVector3(1,0,0));
-            vecs2.push_back(btVector3(0,1,0));
-            vecs3.push_back(btVector3(0,0,1));
-            m_projections.push_back(btReducedVector(dof, indices, vecs1));
-            m_projections.push_back(btReducedVector(dof, indices, vecs2));
-            m_projections.push_back(btReducedVector(dof, indices, vecs3));
-        }
-        for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
-        {
-            int index = m_nodeRigidConstraints[i][j].m_node->index;
-            m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
-            btAlignedObjectArray<int> indices;
-            indices.push_back(index);
-            btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3;
-            if (m_nodeRigidConstraints[i][j].m_static)
-            {
-                vecs1.push_back(btVector3(1,0,0));
-                vecs2.push_back(btVector3(0,1,0));
-                vecs3.push_back(btVector3(0,0,1));
-                m_projections.push_back(btReducedVector(dof, indices, vecs1));
-                m_projections.push_back(btReducedVector(dof, indices, vecs2));
-                m_projections.push_back(btReducedVector(dof, indices, vecs3));
-            }
-            else
-            {
-                vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal);
-                m_projections.push_back(btReducedVector(dof, indices, vecs1));
-            }
-        }
-        for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
-        {
-            const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
+	int dof = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		dof += m_softBodies[i]->m_nodes.size();
+	}
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			continue;
+		}
+		for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+		{
+			int index = m_staticConstraints[i][j].m_node->index;
+			m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
+			btAlignedObjectArray<int> indices;
+			btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
+			indices.push_back(index);
+			vecs1.push_back(btVector3(1, 0, 0));
+			vecs2.push_back(btVector3(0, 1, 0));
+			vecs3.push_back(btVector3(0, 0, 1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs2));
+			m_projections.push_back(btReducedVector(dof, indices, vecs3));
+		}
+
+		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
+		{
+			int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
+			m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
+			btAlignedObjectArray<int> indices;
+			btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
+			indices.push_back(index);
+			vecs1.push_back(btVector3(1, 0, 0));
+			vecs2.push_back(btVector3(0, 1, 0));
+			vecs3.push_back(btVector3(0, 0, 1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs2));
+			m_projections.push_back(btReducedVector(dof, indices, vecs3));
+		}
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+		{
+			int index = m_nodeRigidConstraints[i][j].m_node->index;
+			m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
+			btAlignedObjectArray<int> indices;
+			indices.push_back(index);
+			btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
+			if (m_nodeRigidConstraints[i][j].m_static)
+			{
+				vecs1.push_back(btVector3(1, 0, 0));
+				vecs2.push_back(btVector3(0, 1, 0));
+				vecs3.push_back(btVector3(0, 0, 1));
+				m_projections.push_back(btReducedVector(dof, indices, vecs1));
+				m_projections.push_back(btReducedVector(dof, indices, vecs2));
+				m_projections.push_back(btReducedVector(dof, indices, vecs3));
+			}
+			else
+			{
+				vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal);
+				m_projections.push_back(btReducedVector(dof, indices, vecs1));
+			}
+		}
+		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+		{
+			const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
 			btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
-            btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
-            for (int k = 0; k < 3; ++k)
-            {
-                face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
-            }
+			btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
+			for (int k = 0; k < 3; ++k)
+			{
+				face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
+			}
 			if (m_faceRigidConstraints[i][j].m_static)
 			{
 				for (int l = 0; l < 3; ++l)
 				{
-					
 					btReducedVector rv(dof);
 					for (int k = 0; k < 3; ++k)
 					{
 						rv.m_indices.push_back(face->m_n[k]->index);
-						btVector3 v(0,0,0);
+						btVector3 v(0, 0, 0);
 						v[l] = bary[k];
 						rv.m_vecs.push_back(v);
-                        rv.sort();
+						rv.sort();
 					}
 					m_projections.push_back(rv);
 				}
@@ -431,121 +427,134 @@ void btDeformableContactProjection::setProjection()
 				{
 					rv.m_indices.push_back(face->m_n[k]->index);
 					rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal);
-                    rv.sort();
+					rv.sort();
 				}
 				m_projections.push_back(rv);
 			}
 		}
-    }
-    btModifiedGramSchmidt<btReducedVector> mgs(m_projections);
-    mgs.solve();
-    m_projections = mgs.m_out;
+	}
+	btModifiedGramSchmidt<btReducedVector> mgs(m_projections);
+	mgs.solve();
+	m_projections = mgs.m_out;
 #endif
 }
 
 void btDeformableContactProjection::checkConstraints(const TVStack& x)
 {
-    for (int i = 0; i < m_lagrangeMultipliers.size(); ++i)
-    {
-        btVector3 d(0,0,0);
-        const LagrangeMultiplier& lm = m_lagrangeMultipliers[i];
-        for (int j = 0; j < lm.m_num_constraints; ++j)
-        {
-            for (int k = 0; k < lm.m_num_nodes; ++k)
-            {
-                d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
-            }
-        }
-        printf("d = %f, %f, %f\n",d[0],d[1],d[2]);
-    }
+	for (int i = 0; i < m_lagrangeMultipliers.size(); ++i)
+	{
+		btVector3 d(0, 0, 0);
+		const LagrangeMultiplier& lm = m_lagrangeMultipliers[i];
+		for (int j = 0; j < lm.m_num_constraints; ++j)
+		{
+			for (int k = 0; k < lm.m_num_nodes; ++k)
+			{
+				d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
+			}
+		}
+		//		printf("d = %f, %f, %f\n", d[0], d[1], d[2]);
+		//        printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]);
+	}
 }
 
 void btDeformableContactProjection::setLagrangeMultiplier()
 {
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        if (!psb->isActive())
-        {
-            continue;
-        }
-        for (int j = 0; j < m_staticConstraints[i].size(); ++j)
-        {
-            int index = m_staticConstraints[i][j].m_node->index;
-            m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
-            LagrangeMultiplier lm;
-            lm.m_num_nodes = 1;
-            lm.m_indices[0] = index;
-            lm.m_weights[0] = 1.0;
-            lm.m_num_constraints = 3;
-            lm.m_dirs[0] = btVector3(1,0,0);
-            lm.m_dirs[1] = btVector3(0,1,0);
-            lm.m_dirs[2] = btVector3(0,0,1);
-            m_lagrangeMultipliers.push_back(lm);
-        }
-        for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
-        {
-            int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
-            m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
-            LagrangeMultiplier lm;
-            lm.m_num_nodes = 1;
-            lm.m_indices[0] = index;
-            lm.m_weights[0] = 1.0;
-            lm.m_num_constraints = 3;
-            lm.m_dirs[0] = btVector3(1,0,0);
-            lm.m_dirs[1] = btVector3(0,1,0);
-            lm.m_dirs[2] = btVector3(0,0,1);
-            m_lagrangeMultipliers.push_back(lm);
-        }
-        for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
-        {
-            int index = m_nodeRigidConstraints[i][j].m_node->index;
-            m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
-            LagrangeMultiplier lm;
-            lm.m_num_nodes = 1;
-            lm.m_indices[0] = index;
-            lm.m_weights[0] = 1.0;
-            if (m_nodeRigidConstraints[i][j].m_static)
-            {
-                lm.m_num_constraints = 3;
-                lm.m_dirs[0] = btVector3(1,0,0);
-                lm.m_dirs[1] = btVector3(0,1,0);
-                lm.m_dirs[2] = btVector3(0,0,1);
-            }
-            else
-            {
-                lm.m_num_constraints = 1;
-                lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal;
-            }
-            m_lagrangeMultipliers.push_back(lm);
-        }
-        for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
-        {
-            const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
-			
-            btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
-            btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			continue;
+		}
+		for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+		{
+			int index = m_staticConstraints[i][j].m_node->index;
+			m_staticConstraints[i][j].m_node->m_constrained = true;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 1;
+			lm.m_indices[0] = index;
+			lm.m_weights[0] = 1.0;
+			lm.m_num_constraints = 3;
+			lm.m_dirs[0] = btVector3(1, 0, 0);
+			lm.m_dirs[1] = btVector3(0, 1, 0);
+			lm.m_dirs[2] = btVector3(0, 0, 1);
+			m_lagrangeMultipliers.push_back(lm);
+		}
+		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
+		{
+			int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
+			m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 1;
+			lm.m_indices[0] = index;
+			lm.m_weights[0] = 1.0;
+			lm.m_num_constraints = 3;
+			lm.m_dirs[0] = btVector3(1, 0, 0);
+			lm.m_dirs[1] = btVector3(0, 1, 0);
+			lm.m_dirs[2] = btVector3(0, 0, 1);
+			m_lagrangeMultipliers.push_back(lm);
+		}
+
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+		{
+			if (!m_nodeRigidConstraints[i][j].m_binding)
+			{
+				continue;
+			}
+			int index = m_nodeRigidConstraints[i][j].m_node->index;
+			m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 1;
+			lm.m_indices[0] = index;
+			lm.m_weights[0] = 1.0;
+			if (m_nodeRigidConstraints[i][j].m_static)
+			{
+				lm.m_num_constraints = 3;
+				lm.m_dirs[0] = btVector3(1, 0, 0);
+				lm.m_dirs[1] = btVector3(0, 1, 0);
+				lm.m_dirs[2] = btVector3(0, 0, 1);
+			}
+			else
+			{
+				lm.m_num_constraints = 1;
+				lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal;
+			}
+			m_lagrangeMultipliers.push_back(lm);
+		}
+
+		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+		{
+			if (!m_faceRigidConstraints[i][j].m_binding)
+			{
+				continue;
+			}
+			btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
+
+			btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
 			LagrangeMultiplier lm;
 			lm.m_num_nodes = 3;
-			for (int k = 0; k<3; ++k)
+
+			for (int k = 0; k < 3; ++k)
 			{
-				face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
+				face->m_n[k]->m_constrained = true;
 				lm.m_indices[k] = face->m_n[k]->index;
 				lm.m_weights[k] = bary[k];
 			}
-            if (m_faceRigidConstraints[i][j].m_static)
-            {
+			if (m_faceRigidConstraints[i][j].m_static)
+			{
+				face->m_pcontact[3] = 1;
 				lm.m_num_constraints = 3;
-				lm.m_dirs[0] = btVector3(1,0,0);
-				lm.m_dirs[1] = btVector3(0,1,0);
-				lm.m_dirs[2] = btVector3(0,0,1);
+				lm.m_dirs[0] = btVector3(1, 0, 0);
+				lm.m_dirs[1] = btVector3(0, 1, 0);
+				lm.m_dirs[2] = btVector3(0, 0, 1);
 			}
 			else
 			{
+				face->m_pcontact[3] = 0;
 				lm.m_num_constraints = 1;
 				lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
 			}
-            m_lagrangeMultipliers.push_back(lm);
+			m_lagrangeMultipliers.push_back(lm);
 		}
 	}
 }
@@ -562,7 +571,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 			if (node->m_im != 0)
 			{
 				int index = node->index;
-				f[index] += constraint.getDv(node)* (1./node->m_im);
+				f[index] += constraint.getDv(node) * (1. / node->m_im);
 			}
 		}
 		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
@@ -575,7 +584,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 				if (node->m_im != 0)
 				{
 					int index = node->index;
-					f[index] += constraint.getDv(node)* (1./node->m_im);
+					f[index] += constraint.getDv(node) * (1. / node->m_im);
 				}
 			}
 		}
@@ -587,7 +596,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 			if (node->m_im != 0)
 			{
 				int index = node->index;
-				f[index] += constraint.getDv(node)* (1./node->m_im);
+				f[index] += constraint.getDv(node) * (1. / node->m_im);
 			}
 			for (int k = 0; k < 3; ++k)
 			{
@@ -595,7 +604,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 				if (node->m_im != 0)
 				{
 					int index = node->index;
-					f[index] += constraint.getDv(node)* (1./node->m_im);
+					f[index] += constraint.getDv(node) * (1. / node->m_im);
 				}
 			}
 		}
@@ -612,9 +621,8 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 		m_nodeRigidConstraints.resize(N);
 		m_faceRigidConstraints.resize(N);
 		m_deformableConstraints.resize(N);
-		
 	}
-	for (int i = 0 ; i < N; ++i)
+	for (int i = 0; i < N; ++i)
 	{
 		m_staticConstraints[i].clear();
 		m_nodeAnchorConstraints[i].clear();
@@ -623,12 +631,9 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 		m_deformableConstraints[i].clear();
 	}
 #ifndef USE_MGS
-    m_projectionsDict.clear();
+	m_projectionsDict.clear();
 #else
-    m_projections.clear();
+	m_projections.clear();
 #endif
-    m_lagrangeMultipliers.clear();
+	m_lagrangeMultipliers.clear();
 }
-
-
-