solve constraints involving deformable objects according to islands

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -234,10 +234,10 @@ void btDeformableBodySolver::setConstraints()
     m_objective->setConstraints();
 }
 
-btScalar btDeformableBodySolver::solveContactConstraints()
+btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies)
 {
     BT_PROFILE("solveContactConstraints");
-    btScalar maxSquaredResidual = m_objective->m_projection.update();
+    btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies);
     return maxSquaredResidual;
 }
 

src/BulletSoftBody/btDeformableBodySolver.h

@@ -65,7 +65,7 @@ public:
     virtual void solveDeformableConstraints(btScalar solverdt);
     
     // solve the contact between deformable and rigid as well as among deformables
-    btScalar solveContactConstraints();
+    btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies);
     
     // solve the position error  between deformable and rigid as well as among deformables;
     btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);

src/BulletSoftBody/btDeformableContactProjection.cpp

@@ -17,88 +17,93 @@
 #include "btDeformableMultiBodyDynamicsWorld.h"
 #include <algorithm>
 #include <cmath>
-btScalar btDeformableContactProjection::update()
+btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies)
 {
 	btScalar residualSquare = 0;
-
+	for (int i = 0; i < numDeformableBodies; ++i)
-    // node constraints
-    for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
 	{
-        btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index);
+		for (int j = 0; j < m_softBodies.size(); ++j)
-        for (int i = 0; i < constraints.size(); ++i)
 		{
-            btScalar localResidualSquare = constraints[i].solveConstraint();
+			btCollisionObject* psb = m_softBodies[j];
-            residualSquare = btMax(residualSquare, localResidualSquare);
+			if (psb != deformableBodies[i])
+			{
+				continue;
 			}
-    }
+			for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
-    
-    // anchor constraints
-    for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index)
 			{
-        btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index);
+				btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
 				btScalar localResidualSquare = constraint.solveConstraint();
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
-    
+			for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
-    // face constraints
-    for (int index = 0; index < m_allFaceConstraints.size(); ++index)
 			{
-        btDeformableContactConstraint* constraint = m_allFaceConstraints[index];
+				btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
-        btScalar localResidualSquare = constraint->solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint();
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
-    
+			for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
+			{
+				btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveConstraint();
+				residualSquare = btMax(residualSquare, localResidualSquare);
+			}
+			for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
+			{
+				btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveConstraint();
+				residualSquare = btMax(residualSquare, localResidualSquare);
+			}
+		}
+	}
 	return residualSquare;
 }
 
 void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
+{
+	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
 		// node constraints
-    for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
 		{
-        btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index);
+			btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
-        for (int i = 0; i < constraints.size(); ++i)
+			constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
-        {
-            constraints[i].setPenetrationScale(infoGlobal.m_deformable_erp);
 		}
-    }
-    
 		// face constraints
-    for (int index = 0; index < m_allFaceConstraints.size(); ++index)
+		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
 		{
-        btDeformableContactConstraint* constraint = m_allFaceConstraints[index];
+			btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
-        constraint->setPenetrationScale(infoGlobal.m_deformable_erp);
+			constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
+		}
 	}
 }
 
 btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
 {
 	btScalar residualSquare = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
 		// node constraints
-    for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
 		{
-        btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index);
+			btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
-        for (int i = 0; i < constraints.size(); ++i)
+			btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
-        {
-            btScalar localResidualSquare = constraints[i].solveSplitImpulse(infoGlobal);
 			residualSquare = btMax(residualSquare, localResidualSquare);
 		}
-    }
-    
 		// anchor constraints
-    for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index)
+		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
 		{
-        btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index);
+			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);
 		}
 
-    // face constraints
-    for (int index = 0; index < m_allFaceConstraints.size(); ++index)
-    {
-        btDeformableContactConstraint* constraint = m_allFaceConstraints[index];
-        btScalar localResidualSquare = constraint->solveSplitImpulse(infoGlobal);
-        residualSquare = btMax(residualSquare, localResidualSquare);
 	}
 	return residualSquare;
 }
@@ -106,7 +111,6 @@ btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverI
 void btDeformableContactProjection::setConstraints()
 {
 	BT_PROFILE("setConstraints");
-    // set Dirichlet constraint
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
 		btSoftBody* psb = m_softBodies[i];
@@ -114,23 +118,16 @@ void btDeformableContactProjection::setConstraints()
 		{
 			continue;
 		}
+
+		// set Dirichlet constraint
 		for (int j = 0; j < psb->m_nodes.size(); ++j)
 		{
 			if (psb->m_nodes[j].m_im == 0)
 			{
 				btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]);
-                m_staticConstraints.insert(psb->m_nodes[j].index, static_constraint);
+				m_staticConstraints[i].push_back(static_constraint);
 			}
 		}
-    }
-    
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodies[i];
-        if (!psb->isActive())
-        {
-            continue;
-        }
 		
 		// set up deformable anchors
 		for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
@@ -141,13 +138,9 @@ void btDeformableContactProjection::setConstraints()
 			{
 				continue;
 			}
-            
-            if (m_nodeAnchorConstraints.find(anchor.m_node->index) == NULL)
-            {
 			anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
 			btDeformableNodeAnchorConstraint constraint(anchor);
-                m_nodeAnchorConstraints.insert(anchor.m_node->index, constraint);
+			m_nodeAnchorConstraints[i].push_back(constraint);
-            }
 		}
 		
 		// set Deformable Node vs. Rigid constraint
@@ -167,17 +160,7 @@ void btDeformableContactProjection::setConstraints()
 			const btScalar dn = btDot(vr, cti.m_normal);
 			if (dn < SIMD_EPSILON)
 			{
-                if (m_nodeRigidConstraints.find(contact.m_node->index) == NULL)
+				m_nodeRigidConstraints[i].push_back(constraint);
-                {
-                    btAlignedObjectArray<btDeformableNodeRigidContactConstraint> constraintsList;
-                    constraintsList.push_back(constraint);
-                    m_nodeRigidConstraints.insert(contact.m_node->index, constraintsList);
-                }
-                else
-                {
-                    btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[contact.m_node->index];
-                    constraintsList.push_back(constraint);
-                }
 			}
 		}
 		
@@ -190,39 +173,15 @@ void btDeformableContactProjection::setConstraints()
 			{
 				continue;
 			}
-            btDeformableFaceRigidContactConstraint* constraint = new btDeformableFaceRigidContactConstraint(contact);
+			btDeformableFaceRigidContactConstraint constraint(contact);
-            btVector3 va = constraint->getVa();
+			btVector3 va = constraint.getVa();
-            btVector3 vb = constraint->getVb();
+			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_allFaceConstraints.push_back(constraint);
+				m_faceRigidConstraints[i].push_back(constraint);
-                // add face constraints to each of the nodes
-                for (int k = 0; k < 3; ++k)
-                {
-                    btSoftBody::Node* node = contact.m_face->m_n[k];
-                    // static node does not need to own face/rigid constraint
-                    if (node->m_im != 0)
-                    {
-                        if (m_faceRigidConstraints.find(node->index) == NULL)
-                        {
-                            btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> constraintsList;
-                            constraintsList.push_back(constraint);
-                            m_faceRigidConstraints.insert(node->index, constraintsList);
-                        }
-                        else
-                        {
-                            btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[node->index];
-                            constraintsList.push_back(constraint);
-                        }
-                    }
-                }
-            }
-            else
-            {
-                delete constraint;
 			}
 		}
 		
@@ -231,55 +190,14 @@ void btDeformableContactProjection::setConstraints()
 		{
 			const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j];
 
-            btDeformableFaceNodeContactConstraint* constraint = new btDeformableFaceNodeContactConstraint(contact);
+			btDeformableFaceNodeContactConstraint constraint(contact);
-            btVector3 va = constraint->getVa();
+			btVector3 va = constraint.getVa();
-            btVector3 vb = constraint->getVb();
+			btVector3 vb = constraint.getVb();
 			const btVector3 vr = vb - va;
 			const btScalar dn = btDot(vr, contact.m_normal);
 			if (dn > -SIMD_EPSILON)
 			{
-                btSoftBody::Node* node = contact.m_node;
+				m_deformableConstraints[i].push_back(constraint);
-                btSoftBody::Face* face = contact.m_face;
-                m_allFaceConstraints.push_back(constraint);
-                if (node->m_im != 0)
-                {
-                    if (m_deformableConstraints.find(node->index) == NULL)
-                    {
-                        btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> constraintsList;
-                        constraintsList.push_back(constraint);
-                        m_deformableConstraints.insert(node->index, constraintsList);
-                    }
-                    else
-                    {
-                        btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[node->index];
-                        constraintsList.push_back(constraint);
-                    }
-                }
-                    
-                // add face constraints to each of the nodes
-                for (int k = 0; k < 3; ++k)
-                {
-                    btSoftBody::Node* node = face->m_n[k];
-                    // static node does not need to own face/rigid constraint
-                    if (node->m_im != 0)
-                    {
-                        if (m_deformableConstraints.find(node->index) == NULL)
-                        {
-                            btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> constraintsList;
-                            constraintsList.push_back(constraint);
-                            m_deformableConstraints.insert(node->index, constraintsList);
-                        }
-                        else
-                        {
-                            btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[node->index];
-                            constraintsList.push_back(constraint);
-                        }
-                    }
-                }
-            }
-            else
-            {
-                delete constraint;
 			}
 		}
 	}
@@ -303,7 +221,7 @@ void btDeformableContactProjection::project(TVStack& x)
 			btVector3 dir0 = projectionDirs[0];
 			btVector3 dir1 = projectionDirs[1];
 			btVector3 free_dir = btCross(dir0, dir1);
-            if (free_dir.norm() < SIMD_EPSILON)
+			if (free_dir.safeNorm() < SIMD_EPSILON)
 			{
 				x[i] -= x[i].dot(dir0) * dir0;
 				x[i] -= x[i].dot(dir1) * dir1;
@@ -325,6 +243,10 @@ void btDeformableContactProjection::project(TVStack& x)
 
 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];
@@ -332,193 +254,228 @@ void btDeformableContactProjection::setProjection()
 		{
 			continue;
 		}
-        for (int j = 0; j < psb->m_nodes.size(); ++j)
+		for (int j = 0; j < m_staticConstraints[i].size(); ++j)
 		{
-            int index = psb->m_nodes[j].index;
+			int index = m_staticConstraints[i][j].m_node->index;
-            bool hasConstraint = false;
+			if (m_projectionsDict.find(index) == NULL)
-            bool existStaticConstraint = false;
-            btVector3 averagedNormal(0,0,0);
-            btAlignedObjectArray<btVector3> normals;
-            if (m_staticConstraints.find(index) != NULL || m_nodeAnchorConstraints.find(index) != NULL)
 			{
-                existStaticConstraint = true;
+				m_projectionsDict.insert(index, units);
-                hasConstraint = true;
-            }
-            
-            // accumulate normals from Deformable Node vs. Rigid constraints
-            if (!existStaticConstraint && m_nodeRigidConstraints.find(index) != NULL)
-            {
-                hasConstraint = true;
-                btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[index];
-                for (int k = 0; k < constraintsList.size(); ++k)
-                {
-                    if (constraintsList[k].m_static)
-                    {
-                        existStaticConstraint = true;
-                        break;
-                    }
-                    const btVector3& local_normal = constraintsList[k].m_normal;
-                    normals.push_back(local_normal);
-                    averagedNormal += local_normal;
-                }
-            }
-        
-            // accumulate normals from Deformable Face vs. Rigid constraints
-            if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL)
-            {
-                hasConstraint = true;
-                btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[index];
-                for (int k = 0; k < constraintsList.size(); ++k)
-                {
-                    if (constraintsList[k]->m_static)
-                    {
-                        existStaticConstraint = true;
-                        break;
-                    }
-                    const btVector3& local_normal = constraintsList[k]->m_normal;
-                    normals.push_back(local_normal);
-                    averagedNormal += local_normal;
-                }
-            }
-            
-            // accumulate normals from Deformable Node vs. Deformable Face constraints
-            if (!existStaticConstraint && m_deformableConstraints.find(index) != NULL)
-            {
-                hasConstraint = true;
-                btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[index];
-                for (int k = 0; k < constraintsList.size(); ++k)
-                {
-                    if (constraintsList[k]->m_static)
-                    {
-                        existStaticConstraint = true;
-                        break;
-                    }
-                    const btVector3& local_normal = constraintsList[k]->m_normal;
-                    normals.push_back(local_normal);
-                    averagedNormal += local_normal;
-                }
-            }
-            
-            
-            // build projections
-            if (!hasConstraint)
-            {
-                continue;
-            }
-            btAlignedObjectArray<btVector3> projections;
-            if (existStaticConstraint)
-            {
-                projections.push_back(btVector3(1,0,0));
-                projections.push_back(btVector3(0,1,0));
-                projections.push_back(btVector3(0,0,1));
 			}
 			else
 			{
-                bool averageExists = (averagedNormal.length2() > SIMD_EPSILON);
+				btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-                averagedNormal = averageExists ? averagedNormal.normalized() : btVector3(0,0,0);
+				for (int k = 0; k < 3; ++k)
-                if (averageExists)
 				{
-                    projections.push_back(averagedNormal);
+					projections.push_back(units[k]);
 				}
-                for (int k = 0; k < normals.size(); ++k)
+			}
+		}
+		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
 		{
-                    const btVector3& local_normal = normals[k];
+			int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
-                    // add another projection direction if it deviates from the average by more than about 15 degrees
+			if (m_projectionsDict.find(index) == NULL)
-                    if (!averageExists || btAngle(averagedNormal, local_normal) > 0.25)
 			{
-                        projections.push_back(local_normal);
+				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];
+					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;
+			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);
+					}
+				}
+			}
+			
+			const btSoftBody::Node* node = m_deformableConstraints[i][j].m_node;
+			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);
+				}
+			}
+		}
 	}
 }
 
 
 void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 {
-    // loop over constraints
+	for (int i = 0; i < m_softBodies.size(); ++i)
-    for (int i = 0; i < f.size(); ++i)
 	{
-        if (m_projectionsDict.find(i) != NULL)
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
-        {
-            // doesn't need to add friction force for fully constrained vertices
-            btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict[i];
-            if (projectionDirs.size() >= 3)
-            {
-                continue;
-            }
-        }
-        
-        // add friction contribution from Face vs. Node
-        if (m_nodeRigidConstraints.find(i) != NULL)
-        {
-            btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[i];
-            for (int j = 0; j < constraintsList.size(); ++j)
-            {
-                const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j];
-                btSoftBody::Node* node = constraint.getContact()->m_node;
-                
-                // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway.
-                f[i] += constraint.getDv(node)* (1./node->m_im);
-            }
-        }
-
-        // add friction contribution from Face vs. Rigid
-        if (m_faceRigidConstraints.find(i) != NULL)
-        {
-            btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[i];
-            for (int j = 0; j < constraintsList.size(); ++j)
-            {
-                const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j];
-                btSoftBody::Face* face = constraint->getContact()->m_face;
-                
-                // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway.
-                for (int k = 0; k < 3; ++k)
-                {
-                    if (face->m_n[k]->index == i)
-                    {
-                        if (face->m_n[k]->m_im != 0)
-                        {
-                            f[i] += constraint->getDv(face->m_n[k])* (1./face->m_n[k]->m_im);
-                        }
-                        break;
-                    }
-                }
-            }
-        }
-        
-        if (m_deformableConstraints.find(i) != NULL)
-        {
-            btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[i];
-            for (int j = 0; j < constraintsList.size(); ++j)
-            {
-                const btDeformableFaceNodeContactConstraint* constraint = constraintsList[j];
-                btSoftBody::Face* face = constraint->getContact()->m_face;
-                btSoftBody::Node* node = constraint->getContact()->m_node;
-                
-                // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway.
-                if (node->index == i)
 		{
+			const btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
+			const btSoftBody::Node* node = constraint.m_node;
 			if (node->m_im != 0)
 			{
-                        f[i] += constraint->getDv(node)*(1./node->m_im);
+				int index = node->index;
+				f[index] += constraint.getDv(node)* (1./node->m_im);
 			}
 		}
-                else
+		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
 		{
+			const btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
+			const btSoftBody::Face* face = constraint.getContact()->m_face;
 			for (int k = 0; k < 3; ++k)
 			{
-                        if (face->m_n[k]->index == i)
+				const btSoftBody::Node* node = face->m_n[k];
+				if (node->m_im != 0)
 				{
-                            if (face->m_n[k]->m_im != 0)
+					int index = node->index;
+					f[index] += constraint.getDv(node)* (1./node->m_im);
+				}
+			}
+		}
+		for (int j = 0; j < m_deformableConstraints[i].size(); ++j)
 		{
-                                f[i] += constraint->getDv(face->m_n[k])* (1./face->m_n[k]->m_im);
+			const btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[i][j];
-                            }
+			const btSoftBody::Face* face = constraint.getContact()->m_face;
-                            break;
+			const btSoftBody::Node* node = constraint.getContact()->m_node;
-                        }
+			if (node->m_im != 0)
+			{
+				int index = node->index;
+				f[index] += constraint.getDv(node)* (1./node->m_im);
 			}
+			for (int k = 0; k < 3; ++k)
+			{
+				const btSoftBody::Node* node = face->m_n[k];
+				if (node->m_im != 0)
+				{
+					int index = node->index;
+					f[index] += constraint.getDv(node)* (1./node->m_im);
 				}
 			}
 		}
@@ -527,17 +484,25 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 
 void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 {
-    m_staticConstraints.clear();
+	int N = m_softBodies.size();
-    m_nodeAnchorConstraints.clear();
+	if (nodeUpdated)
-    m_nodeRigidConstraints.clear();
-    m_faceRigidConstraints.clear();
-    m_deformableConstraints.clear();
-    m_projectionsDict.clear();
-    for (int i = 0; i < m_allFaceConstraints.size(); ++i)
 	{
-        delete m_allFaceConstraints[i];
+		m_staticConstraints.resize(N);
+		m_nodeAnchorConstraints.resize(N);
+		m_nodeRigidConstraints.resize(N);
+		m_faceRigidConstraints.resize(N);
+		m_deformableConstraints.resize(N);
+		
 	}
-    m_allFaceConstraints.clear();
+	for (int i = 0 ; i < N; ++i)
+	{
+		m_staticConstraints[i].clear();
+		m_nodeAnchorConstraints[i].clear();
+		m_nodeRigidConstraints[i].clear();
+		m_faceRigidConstraints[i].clear();
+		m_deformableConstraints[i].clear();
+	}
+	m_projectionsDict.clear();
 }
 
 

src/BulletSoftBody/btDeformableContactProjection.h

@@ -28,16 +28,16 @@ public:
     typedef btAlignedObjectArray<btVector3> TVStack;
     btAlignedObjectArray<btSoftBody *>& m_softBodies;
     
-    // map from node index to static constraint
+//    // map from node index to static constraint
-    btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
+//    btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
-    // map from node index to node rigid constraint
+//    // map from node index to node rigid constraint
-    btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
+//    btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
-    // map from node index to face rigid constraint
+//    // map from node index to face rigid constraint
-    btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
+//    btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
-    // map from node index to deformable constraint
+//    // map from node index to deformable constraint
-    btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
+//    btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
-    // map from node index to node anchor constraint
+//    // map from node index to node anchor constraint
-    btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
+//    btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
 	
     // all constraints involving face
     btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
@@ -45,6 +45,17 @@ public:
     // map from node index to projection directions
     btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
 	
+	// map from node index to static constraint
+	btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints;
+	// map from node index to node rigid constraint
+	btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
+	// map from node index to face rigid constraint
+	btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceRigidContactConstraint> > m_faceRigidConstraints;
+	// map from node index to deformable constraint
+	btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceNodeContactConstraint> > m_deformableConstraints;
+	// map from node index to node anchor constraint
+	btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints;
+    
     btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
     : m_softBodies(softBodies)
     {
@@ -61,7 +72,7 @@ public:
     virtual void applyDynamicFriction(TVStack& f);
     
     // update and solve the constraints
-    virtual btScalar update();
+    virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies);
     
     // solve the position error using split impulse
     virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -22,32 +22,8 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
     {
         ///this is a special step to resolve penetrations (just for contacts)
         solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
-//        
-//        int maxMotorIterations = 150;
-//        for (int iteration = 0; iteration < maxMotorIterations; ++iteration)
-//        {
-//            btScalar motorResidual = 0;
-//            for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
-//            {
-//                int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
-//                
-//                btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
-//                
-//                btScalar residual = resolveSingleConstraintRowGeneric(constraint);
-//                motorResidual = btMax(motorResidual, residual * residual);
-//                if (constraint.m_multiBodyA)
-//                    constraint.m_multiBodyA->setPosUpdated(false);
-//                if (constraint.m_multiBodyB)
-//                    constraint.m_multiBodyB->setPosUpdated(false);
-//            }
-//            if (motorResidual < infoGlobal.m_leastSquaresResidualThreshold)
-//            {
-//                break;
-//            }
-//        }
 
         int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
-        maxIterations = 500;
         for (int iteration = 0; iteration < maxIterations; iteration++)
         {
             // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body
@@ -56,7 +32,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
             m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
             // solver body velocity -> rigid body velocity
             solverBodyWriteBack(infoGlobal);
-            btScalar deformableResidual = m_deformableSolver->solveContactConstraints();
+            btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies);
             // update rigid body velocity in rigid/deformable contact
             m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
             // solver body velocity <- rigid body velocity