only call buildIslands once for multibody in each timestep

6 files changed, 20 insertions, 10 deletions

diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
index 11fa97cdd..57e1bb494 100644
--- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
+++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
@@ -233,7 +233,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
 				//				printf("error in island management\n");
 			}
 
-//            btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+            btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 			if (colObj0->getIslandTag() == islandId)
 			{
 				if (colObj0->getActivationState() == ACTIVE_TAG ||
@@ -257,7 +257,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
 					//					printf("error in island management\n");
 				}
 
-//                btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+                btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 
 				if (colObj0->getIslandTag() == islandId)
 				{
@@ -278,7 +278,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
 					//					printf("error in island management\n");
 				}
 
-//                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 
 
 				if (colObj0->getIslandTag() == islandId)
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index d1ac5e26f..2e6dbc440 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -428,14 +428,18 @@ void btMultiBodyDynamicsWorld::forwardKinematics()
 void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
     solveExternalForces(solverInfo);
+    buildIslands();
     solveInternalConstraints(solverInfo);
 }
 
+void btMultiBodyDynamicsWorld::buildIslands()
+{
+    m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
+}
+
 void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo)
 {
 	/// solve all the constraints for this island
-	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
-
 	m_solverMultiBodyIslandCallback->processConstraints();
 
 	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
index e82c17b31..04707bf2d 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
@@ -116,6 +116,7 @@ public:
     
     virtual void solveExternalForces(btContactSolverInfo& solverInfo);
     virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
+    void buildIslands();
 
 };
 #endif  //BT_MULTIBODY_DYNAMICS_WORLD_H
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
index 9ffd63a3c..7b084af0e 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
@@ -128,3 +128,11 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack
         }
     }
 }
+
+//set constraints as projections
+void btDeformableBackwardEulerObjective::setConstraints()
+{
+    // build islands for multibody solve
+    m_world->btMultiBodyDynamicsWorld::buildIslands();
+    projection.setConstraints();
+}
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
index e22b42b0b..a736315b7 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
@@ -73,10 +73,7 @@ public:
     void updateVelocity(const TVStack& dv);
     
     //set constraints as projections
-    void setConstraints()
-    {
-        projection.setConstraints();
-    }
+    void setConstraints();
     
     // update the projections and project the residual
     void project(TVStack& r)
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index e3009dcbb..a1616a460 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -31,7 +31,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt)
     backupVelocity();
     
     m_objective->computeResidual(solverdt, m_residual);
-//   m_objective->initialGuess(m_dv, m_residual);
+    
     computeStep(m_dv, m_residual);
     updateVelocity();
 }