Update to bullet master (2.90)

thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp

@@ -17,7 +17,7 @@
 #include "btDeformableMultiBodyDynamicsWorld.h"
 #include <algorithm>
 #include <cmath>
-btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies)
+btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 {
 	btScalar residualSquare = 0;
 	for (int i = 0; i < numDeformableBodies; ++i)
@@ -32,25 +32,25 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod
 			for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
 			{
 				btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 			for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
 			{
 				btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 			for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
 			{
 				btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 			for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
 			{
 				btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 		}
@@ -77,39 +77,8 @@ void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo&
 	}
 }
 
-btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
-{
-	btScalar residualSquare = 0;
-	for (int i = 0; i < m_softBodies.size(); ++i)
-	{
-		// node constraints
-		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
-		{
-			btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
-			btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
-			residualSquare = btMax(residualSquare, localResidualSquare);
-		}
-		// anchor constraints
-		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
-		{
-			btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[i][j];
-			btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
-			residualSquare = btMax(residualSquare, localResidualSquare);
-		}
-		// face constraints
-		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
-		{
-			btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
-			btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
-			residualSquare = btMax(residualSquare, localResidualSquare);
-		}
-
-	}
-	return residualSquare;
-}
-
-void btDeformableContactProjection::setConstraints()
-{
+void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
+{  
 	BT_PROFILE("setConstraints");
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
@@ -124,7 +93,7 @@ void btDeformableContactProjection::setConstraints()
 		{
 			if (psb->m_nodes[j].m_im == 0)
 			{
-				btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]);
+				btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal);
 				m_staticConstraints[i].push_back(static_constraint);
 			}
 		}
@@ -139,7 +108,7 @@ void btDeformableContactProjection::setConstraints()
 				continue;
 			}
 			anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
-			btDeformableNodeAnchorConstraint constraint(anchor);
+			btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal);
 			m_nodeAnchorConstraints[i].push_back(constraint);
 		}
 		
@@ -152,7 +121,7 @@ void btDeformableContactProjection::setConstraints()
 			{
 				continue;
 			}
-			btDeformableNodeRigidContactConstraint constraint(contact);
+			btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
 			btVector3 va = constraint.getVa();
 			btVector3 vb = constraint.getVb();
 			const btVector3 vr = vb - va;
@@ -173,7 +142,7 @@ void btDeformableContactProjection::setConstraints()
 			{
 				continue;
 			}
-			btDeformableFaceRigidContactConstraint constraint(contact);
+			btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
 			btVector3 va = constraint.getVa();
 			btVector3 vb = constraint.getVb();
 			const btVector3 vr = vb - va;
@@ -184,253 +153,404 @@ void btDeformableContactProjection::setConstraints()
 				m_faceRigidConstraints[i].push_back(constraint);
 			}
 		}
-		
-		// set Deformable Face vs. Deformable Node constraint
-		for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j)
-		{
-			const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j];
-
-			btDeformableFaceNodeContactConstraint constraint(contact);
-			btVector3 va = constraint.getVa();
-			btVector3 vb = constraint.getVb();
-			const btVector3 vr = vb - va;
-			const btScalar dn = btDot(vr, contact.m_normal);
-			if (dn > -SIMD_EPSILON)
-			{
-				m_deformableConstraints[i].push_back(constraint);
-			}
-		}
 	}
 }
 
 void btDeformableContactProjection::project(TVStack& x)
 {
-	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;
-		}
-	}
+#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;
+        }
+    }
+#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];
+    }
+#endif
 }
 
 void 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;
-			if (m_projectionsDict.find(index) == NULL)
+#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);
+                    }
+                }
+            }
+        }
+    }
+#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;
+			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);
+            }
+			if (m_faceRigidConstraints[i][j].m_static)
 			{
-				m_projectionsDict.insert(index, units);
-			}
-			else
-			{
-				btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-				for (int k = 0; k < 3; ++k)
+				for (int l = 0; l < 3; ++l)
 				{
-					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;
-			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;
-			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];
+					
+					btReducedVector rv(dof);
 					for (int k = 0; k < 3; ++k)
 					{
-						projections.push_back(units[k]);
+						rv.m_indices.push_back(face->m_n[k]->index);
+						btVector3 v(0,0,0);
+						v[l] = bary[k];
+						rv.m_vecs.push_back(v);
+                        rv.sort();
 					}
+					m_projections.push_back(rv);
 				}
 			}
 			else
 			{
-				if (m_projectionsDict.find(index) == NULL)
+				btReducedVector rv(dof);
+				for (int k = 0; k < 3; ++k)
 				{
-					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);
+					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();
 				}
+				m_projections.push_back(rv);
 			}
 		}
-		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
-		{
-			const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
-			for (int k = 0; k < 3; ++k)
-			{
-				const 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 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);
-					}
-				}
-			}
-		}
-		for (int j = 0; j < m_deformableConstraints[i].size(); ++j)
-		{
-			const btSoftBody::Face* face = m_deformableConstraints[i][j].m_face;
-			for (int k = 0; k < 3; ++k)
-			{
-				const btSoftBody::Node* node = face->m_n[k];
-				int index = node->index;
-				if (m_deformableConstraints[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_deformableConstraints[i][j].m_normal);
-						m_projectionsDict.insert(index, projections);
-					}
-					else
-					{
-						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-						projections.push_back(m_deformableConstraints[i][j].m_normal);
-					}
-				}
-			}
+    }
+    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]);
+    }
+}
+
+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;
 			
-			const btSoftBody::Node* node = m_deformableConstraints[i][j].m_node;
-			int index = node->index;
-			if (m_deformableConstraints[i][j].m_static)
+            btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
+            btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 3;
+			for (int k = 0; k<3; ++k)
 			{
-				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]);
-					}
-				}
+				face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
+				lm.m_indices[k] = face->m_n[k]->index;
+				lm.m_weights[k] = bary[k];
+			}
+            if (m_faceRigidConstraints[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
 			{
-				if (m_projectionsDict.find(index) == NULL)
-				{
-					btAlignedObjectArray<btVector3> projections;
-					projections.push_back(m_deformableConstraints[i][j].m_normal);
-					m_projectionsDict.insert(index, projections);
-				}
-				else
-				{
-					btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-					projections.push_back(m_deformableConstraints[i][j].m_normal);
-				}
+				lm.m_num_constraints = 1;
+				lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
 			}
+            m_lagrangeMultipliers.push_back(lm);
 		}
 	}
 }
 
-
+//
 void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 {
 	for (int i = 0; i < m_softBodies.size(); ++i)
@@ -502,7 +622,12 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 		m_faceRigidConstraints[i].clear();
 		m_deformableConstraints[i].clear();
 	}
-	m_projectionsDict.clear();
+#ifndef USE_MGS
+    m_projectionsDict.clear();
+#else
+    m_projections.clear();
+#endif
+    m_lagrangeMultipliers.clear();
 }