Merge pull request #2550 from xhan0619/master

Group deformable constraint solves by islands

examples/ExampleBrowser/ExampleEntries.cpp

@@ -303,6 +303,7 @@ static ExampleEntry gDefaultExamples[] =
 		ExampleEntry(1, "Rolling friction", "Experiment on multibody rolling friction", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_ROLLING_FRICTION),
 		ExampleEntry(1, "Gripper Grasp", "Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc, eGRIPPER_GRASP),
 		ExampleEntry(1, "Two Point Grasp", "Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP),
+		ExampleEntry(1, "Grasp Deformable Cloth", "Grasp experiment with deformable cloth", GripperGraspExampleCreateFunc, eGRASP_DEFORMABLE_CLOTH),
 		ExampleEntry(1, "One Motor Gripper Grasp", "Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP),
 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
 		ExampleEntry(1, "Grasp Soft Body", "Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY),

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -373,6 +373,112 @@ public:
 				m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
 			}
 
+			if ((m_options & eGRASP_DEFORMABLE_CLOTH) != 0)
+			{
+				m_robotSim.resetSimulation(RESET_USE_DEFORMABLE_WORLD);
+			{
+				SliderParams slider("Vertical velocity", &sGripperVerticalVelocity);
+				slider.m_minVal = -2;
+				slider.m_maxVal = 2;
+				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+			}
+
+			{
+				SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
+				slider.m_minVal = -1;
+				slider.m_maxVal = 1;
+				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+			}
+
+			{
+				b3RobotSimulatorLoadFileResults results;
+				m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", results);
+
+				if (results.m_uniqueObjectIds.size() == 1)
+				{
+					m_gripperIndex = results.m_uniqueObjectIds[0];
+					int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
+					b3Printf("numJoints = %d", numJoints);
+
+					for (int i = 0; i < numJoints; i++)
+					{
+						b3JointInfo jointInfo;
+						m_robotSim.getJointInfo(m_gripperIndex, i, &jointInfo);
+						b3Printf("joint[%d].m_jointName=%s", i, jointInfo.m_jointName);
+					}
+
+					for (int i = 0; i < 8; i++)
+					{
+						b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+						controlArgs.m_maxTorqueValue = 0.0;
+						m_robotSim.setJointMotorControl(m_gripperIndex, i, controlArgs);
+					}
+				}
+			}
+			{
+				b3RobotSimulatorLoadUrdfFileArgs args;
+				args.m_startPosition.setValue(0, 0, -0.2);
+				args.m_startOrientation.setEulerZYX(0, 0, 0);
+				m_robotSim.loadURDF("plane.urdf", args);
+			}
+			m_robotSim.setGravity(btVector3(0, 0, -10));
+
+			m_robotSim.setGravity(btVector3(0, 0, -10));
+			b3RobotSimulatorLoadDeformableBodyArgs args(2, .01, 0.006);
+			args.m_springElasticStiffness = .1;
+			args.m_springDampingStiffness = .0004;
+			args.m_springBendingStiffness = 1;
+			args.m_frictionCoeff = 1;
+			args.m_useSelfCollision = false;
+//			args.m_useFaceContact = true;
+			args.m_useBendingSprings = true;
+			args.m_startPosition.setValue(0, 0, 0);
+			args.m_startOrientation.setValue(0, 0, 1, 1);
+			m_robotSim.loadDeformableBody("flat_napkin.obj", args);
+
+			b3JointInfo revoluteJoint1;
+			revoluteJoint1.m_parentFrame[0] = -0.055;
+			revoluteJoint1.m_parentFrame[1] = 0;
+			revoluteJoint1.m_parentFrame[2] = 0.02;
+			revoluteJoint1.m_parentFrame[3] = 0;
+			revoluteJoint1.m_parentFrame[4] = 0;
+			revoluteJoint1.m_parentFrame[5] = 0;
+			revoluteJoint1.m_parentFrame[6] = 1.0;
+			revoluteJoint1.m_childFrame[0] = 0;
+			revoluteJoint1.m_childFrame[1] = 0;
+			revoluteJoint1.m_childFrame[2] = 0;
+			revoluteJoint1.m_childFrame[3] = 0;
+			revoluteJoint1.m_childFrame[4] = 0;
+			revoluteJoint1.m_childFrame[5] = 0;
+			revoluteJoint1.m_childFrame[6] = 1.0;
+			revoluteJoint1.m_jointAxis[0] = 1.0;
+			revoluteJoint1.m_jointAxis[1] = 0.0;
+			revoluteJoint1.m_jointAxis[2] = 0.0;
+			revoluteJoint1.m_jointType = ePoint2PointType;
+			b3JointInfo revoluteJoint2;
+			revoluteJoint2.m_parentFrame[0] = 0.055;
+			revoluteJoint2.m_parentFrame[1] = 0;
+			revoluteJoint2.m_parentFrame[2] = 0.02;
+			revoluteJoint2.m_parentFrame[3] = 0;
+			revoluteJoint2.m_parentFrame[4] = 0;
+			revoluteJoint2.m_parentFrame[5] = 0;
+			revoluteJoint2.m_parentFrame[6] = 1.0;
+			revoluteJoint2.m_childFrame[0] = 0;
+			revoluteJoint2.m_childFrame[1] = 0;
+			revoluteJoint2.m_childFrame[2] = 0;
+			revoluteJoint2.m_childFrame[3] = 0;
+			revoluteJoint2.m_childFrame[4] = 0;
+			revoluteJoint2.m_childFrame[5] = 0;
+			revoluteJoint2.m_childFrame[6] = 1.0;
+			revoluteJoint2.m_jointAxis[0] = 1.0;
+			revoluteJoint2.m_jointAxis[1] = 0.0;
+			revoluteJoint2.m_jointAxis[2] = 0.0;
+			revoluteJoint2.m_jointType = ePoint2PointType;
+			m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1);
+			m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
+			m_robotSim.setNumSimulationSubSteps(8);
+        }
+
 		if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)
 		{
 			{
@@ -463,6 +569,21 @@ public:
 			}
 		}
         
+		if ((m_options & eGRASP_DEFORMABLE_CLOTH) != 0)
+		{
+			int fingerJointIndices[2] = {0, 1};
+			double fingerTargetVelocities[2] = {sGripperVerticalVelocity, sGripperClosingTargetVelocity};
+			double maxTorqueValues[2] = {250.0, 50.0};
+			for (int i = 0; i < 2; i++)
+			{
+				b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+				controlArgs.m_targetVelocity = fingerTargetVelocities[i];
+				controlArgs.m_maxTorqueValue = maxTorqueValues[i];
+				controlArgs.m_kd = 1.;
+				m_robotSim.setJointMotorControl(m_gripperIndex, fingerJointIndices[i], controlArgs);
+			}
+		}
+
 		if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)
 		{
 			float dt = deltaTime;

examples/RoboticsLearning/GripperGraspExample.h

@@ -23,6 +23,7 @@ enum GripperGraspExampleOptions
 	eONE_MOTOR_GRASP = 4,
 	eGRASP_SOFT_BODY = 8,
 	eSOFTBODY_MULTIBODY_COUPLING = 16,
+	eGRASP_DEFORMABLE_CLOTH = 32,
 };
 
 class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

@@ -89,6 +89,19 @@ void b3RobotSimulatorClientAPI_NoDirect::resetSimulation()
 		m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
 }
 
+void b3RobotSimulatorClientAPI_NoDirect::resetSimulation(int flag)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+	b3SharedMemoryStatusHandle statusHandle;
+	b3SharedMemoryCommandHandle command = b3InitResetSimulationCommand(m_data->m_physicsClientHandle);
+	b3InitResetSimulationSetFlags(command, flag);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+}
+
 bool b3RobotSimulatorClientAPI_NoDirect::canSubmitCommand() const
 {
 	if (!isConnected())
@@ -1151,6 +1164,35 @@ void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileNam
 	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
 }
 
+void b3RobotSimulatorClientAPI_NoDirect::loadDeformableBody(const std::string& fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs& args)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+
+	b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
+	b3LoadSoftBodySetStartPosition(command, args.m_startPosition[0], args.m_startPosition[1], args.m_startPosition[2]);
+	b3LoadSoftBodySetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
+	b3LoadSoftBodySetScale(command, args.m_scale);
+	b3LoadSoftBodySetMass(command, args.m_mass);
+	b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin);
+	if (args.m_NeoHookeanMu > 0)
+	{
+		b3LoadSoftBodyAddNeoHookeanForce(command, args.m_NeoHookeanMu, args.m_NeoHookeanLambda, args.m_NeoHookeanDamping);
+	}
+	if (args.m_springElasticStiffness > 0)
+	{
+		b3LoadSoftBodyAddMassSpringForce(command, args.m_springElasticStiffness, args.m_springDampingStiffness);
+	}
+	b3LoadSoftBodySetSelfCollision(command, args.m_useSelfCollision);
+	b3LoadSoftBodyUseFaceContact(command, args.m_useFaceContact);
+	b3LoadSoftBodySetFrictionCoefficient(command, args.m_frictionCoeff);
+	b3LoadSoftBodyUseBendingSprings(command, args.m_useBendingSprings, args.m_springBendingStiffness);
+	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+}
+
 void b3RobotSimulatorClientAPI_NoDirect::getMouseEvents(b3MouseEventsData* mouseEventsData)
 {
 	mouseEventsData->m_numMouseEvents = 0;

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h

@@ -89,6 +89,64 @@ struct b3RobotSimulatorLoadSoftBodyArgs
 	}
 };
 
+
+struct b3RobotSimulatorLoadDeformableBodyArgs
+{
+	btVector3 m_startPosition;
+	btQuaternion m_startOrientation;
+	double m_scale;
+	double m_mass;
+	double m_collisionMargin;
+	double m_springElasticStiffness;
+	double m_springDampingStiffness;
+	double m_springBendingStiffness;
+	double m_NeoHookeanMu;
+	double m_NeoHookeanLambda;
+	double m_NeoHookeanDamping;
+	bool m_useSelfCollision;
+	bool m_useFaceContact;
+	bool m_useBendingSprings;
+	double m_frictionCoeff;
+
+	b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn, const double &scale, const double &mass, const double &collisionMargin)
+	: m_startPosition(startPos),
+	m_startOrientation(startOrn),
+	m_scale(scale),
+	m_mass(mass),
+	m_collisionMargin(collisionMargin),
+	m_springElasticStiffness(-1),
+	m_springDampingStiffness(-1),
+	m_springBendingStiffness(-1),
+	m_NeoHookeanMu(-1),
+	m_NeoHookeanDamping(-1),
+	m_useSelfCollision(false),
+	m_useFaceContact(false),
+	m_useBendingSprings(false),
+	m_frictionCoeff(0)
+	{
+	}
+
+	b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn)
+	{
+		b3RobotSimulatorLoadSoftBodyArgs(startPos, startOrn, 1.0, 1.0, 0.02);
+	}
+
+	b3RobotSimulatorLoadDeformableBodyArgs()
+	{
+		b3RobotSimulatorLoadSoftBodyArgs(btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1));
+	}
+
+	b3RobotSimulatorLoadDeformableBodyArgs(double scale, double mass, double collisionMargin)
+	: m_startPosition(btVector3(0, 0, 0)),
+	m_startOrientation(btQuaternion(0, 0, 0, 1)),
+	m_scale(scale),
+	m_mass(mass),
+	m_collisionMargin(collisionMargin)
+	{
+	}
+};
+
+
 struct b3RobotSimulatorLoadFileResults
 {
 	btAlignedObjectArray<int> m_uniqueObjectIds;
@@ -535,6 +593,8 @@ public:
 
 	void resetSimulation();
     
+	void resetSimulation(int flag);
+
 	btQuaternion getQuaternionFromEuler(const btVector3 &rollPitchYaw);
 	btVector3 getEulerFromQuaternion(const btQuaternion &quat);
 
@@ -686,6 +746,8 @@ public:
 
 	void loadSoftBody(const std::string &fileName, const struct b3RobotSimulatorLoadSoftBodyArgs &args);
     
+	void loadDeformableBody(const std::string &fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs &args);
+
 	virtual void setGuiHelper(struct GUIHelperInterface *guiHelper);
 	virtual struct GUIHelperInterface *getGuiHelper();
 

src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h

@@ -76,6 +76,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
     btDispatcher* m_dispatcher;
     
     btAlignedObjectArray<btCollisionObject*> m_bodies;
+	btAlignedObjectArray<btCollisionObject*> m_softBodies;
     btAlignedObjectArray<btPersistentManifold*> m_manifolds;
     btAlignedObjectArray<btTypedConstraint*> m_constraints;
     btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
@@ -194,6 +195,10 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 					{
 						m_bodies.push_back(bodies[i]);
 					}
+					else
+					{
+						m_softBodies.push_back(bodies[i]);
+					}
 				}
                 for (i = 0; i < numManifolds; i++)
                     m_manifolds.push_back(manifolds[i]);
@@ -231,6 +236,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
             m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
         }
         m_bodies.resize(0);
+		m_softBodies.resize(0);
         m_manifolds.resize(0);
         m_constraints.resize(0);
         m_multiBodyConstraints.resize(0);

src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h

@@ -0,0 +1,46 @@
+//
+//  DeformableBodyInplaceSolverIslandCallback.h
+//  BulletSoftBody
+//
+//  Created by Xuchen Han on 12/16/19.
+//
+
+#ifndef DeformableBodyInplaceSolverIslandCallback_h
+#define DeformableBodyInplaceSolverIslandCallback_h
+
+struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolverIslandCallback
+{
+	btDeformableMultiBodyConstraintSolver* m_deformableSolver;
+
+	DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver,
+		 btDispatcher* dispatcher)
+	: MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver)
+	{
+	}
+
+
+	virtual void processConstraints(int islandId=-1)
+	{
+		btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
+		btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0;
+		btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
+		btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
+		btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
+
+		//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
+
+		m_deformableSolver->solveDeformableBodyGroup(bodies, m_bodies.size(), softBodies, m_softBodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
+		if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1))
+		{
+			m_deformableSolver->m_analyticsData.m_islandId = islandId;
+			m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+		}
+		m_bodies.resize(0);
+		m_softBodies.resize(0);
+		m_manifolds.resize(0);
+		m_constraints.resize(0);
+		m_multiBodyConstraints.resize(0);
+	}
+};
+
+#endif /* DeformableBodyInplaceSolverIslandCallback_h */

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

@@ -17,7 +17,7 @@
 #include "btDeformableMultiBodyConstraintSolver.h"
 #include <iostream>
 // override the iterations method to include deformable/multibody contact
-btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
 {
     {
         ///this is a special step to resolve penetrations (just for contacts)
@@ -32,7 +32,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
             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
@@ -58,7 +58,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
     return 0.f;
 }
 
-void btDeformableMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
 {
     m_tmpMultiBodyConstraints = multiBodyConstraints;
     m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
@@ -67,7 +67,7 @@ void btDeformableMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObjec
     solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
     
     // overriden
-    solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+    solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
     
     // inherited from MultiBodyConstraintSolver
     solveGroupCacheFriendlyFinish(bodies, numBodies, info);

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h

@@ -36,7 +36,7 @@ btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver
     
 protected:
     // override the iterations method to include deformable/multibody contact
-    virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+//    virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
     
     // write the velocity of the the solver body to the underlying rigid body
     void solverBodyWriteBack(const btContactSolverInfo& infoGlobal);
@@ -46,6 +46,7 @@ protected:
     
     virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
     
+    virtual btScalar solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
 public:
     BT_DECLARE_ALIGNED_ALLOCATOR();
     
@@ -54,7 +55,7 @@ public:
         m_deformableSolver = deformableSolver;
     }
     
-    virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
+    virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
 };
 
 #endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -36,9 +36,36 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed
 
 #include <stdio.h>
 #include "btDeformableMultiBodyDynamicsWorld.h"
+#include "DeformableBodyInplaceSolverIslandCallback.h"
 #include "btDeformableBodySolver.h"
 #include "LinearMath/btQuickprof.h"
 #include "btSoftBodyInternals.h"
+btDeformableMultiBodyDynamicsWorld::btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver)
+: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
+m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
+{
+	m_drawFlags = fDrawFlags::Std;
+	m_drawNodeTree = true;
+	m_drawFaceTree = false;
+	m_drawClusterTree = false;
+	m_sbi.m_broadphase = pairCache;
+	m_sbi.m_dispatcher = dispatcher;
+	m_sbi.m_sparsesdf.Initialize();
+	m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005);
+	m_sbi.m_sparsesdf.Reset();
+	
+	m_sbi.air_density = (btScalar)1.2;
+	m_sbi.water_density = 0;
+	m_sbi.water_offset = 0;
+	m_sbi.water_normal = btVector3(0, 0, 0);
+	m_sbi.m_gravity.setValue(0, -10, 0);
+	m_internalTime = 0.0;
+	m_implicit = false;
+	m_lineSearch = false;
+	m_selfCollision = true;
+	m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
+}
+
 void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
     BT_PROFILE("internalSingleStepSimulation");
@@ -261,7 +288,7 @@ void btDeformableMultiBodyDynamicsWorld::setupConstraints()
         // setup the solver callback
         btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
         btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
-        m_solverMultiBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
+        m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
         
         // build islands
         m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
@@ -290,10 +317,10 @@ void btDeformableMultiBodyDynamicsWorld::sortConstraints()
 void btDeformableMultiBodyDynamicsWorld::solveContactConstraints()
 {
     // process constraints on each island
-    m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
+    m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback);
     
     // process deferred
-    m_solverMultiBodyIslandCallback->processConstraints();
+    m_solverDeformableBodyIslandCallback->processConstraints();
     m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer);
     
     // write joint feedback

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

@@ -29,6 +29,7 @@ typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
 class btDeformableBodySolver;
 class btDeformableLagrangianForce;
 struct MultiBodyInplaceSolverIslandCallback;
+struct DeformableBodyInplaceSolverIslandCallback;
 class btDeformableMultiBodyConstraintSolver;
 
 typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
@@ -49,6 +50,7 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
     bool m_implicit;
     bool m_lineSearch;
     bool m_selfCollision;
+    DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback;
     
     typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
     btSolverCallback m_solverCallback;
@@ -67,30 +69,7 @@ protected:
     void clearGravity();
     
 public:
-    btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0)
+	btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0);
-    : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
-    m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
-    {
-        m_drawFlags = fDrawFlags::Std;
-        m_drawNodeTree = true;
-        m_drawFaceTree = false;
-        m_drawClusterTree = false;
-        m_sbi.m_broadphase = pairCache;
-        m_sbi.m_dispatcher = dispatcher;
-        m_sbi.m_sparsesdf.Initialize();
-        m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005);
-        m_sbi.m_sparsesdf.Reset();
-        
-        m_sbi.air_density = (btScalar)1.2;
-        m_sbi.water_density = 0;
-        m_sbi.water_offset = 0;
-        m_sbi.water_normal = btVector3(0, 0, 0);
-        m_sbi.m_gravity.setValue(0, -10, 0);
-        m_internalTime = 0.0;
-        m_implicit = false;
-        m_lineSearch = false;
-        m_selfCollision = true;
-    }
 
     virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));