separate deformable contact solve by islands WIP

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

@@ -1180,7 +1180,7 @@ void b3RobotSimulatorClientAPI_NoDirect::loadDeformableBody(const std::string& f
 	b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin);
 	b3LoadSoftBodyAddNeoHookeanForce(command, args.m_NeoHookeanMu, args.m_NeoHookeanLambda, args.m_NeoHookeanDamping);
 	b3LoadSoftBodyAddMassSpringForce(command, args.m_springElasticStiffness, args.m_springDampingStiffness);
-	b3LoadSoftBodyUseSelfCollision(command, args.m_useSelfCollision);
+	b3LoadSoftBodySetSelfCollision(command, args.m_useSelfCollision);
 	b3LoadSoftBodyUseFaceContact(command, args.m_useFaceContact);
 	b3LoadSoftBodySetFrictionCoefficient(command, args.m_frictionCoeff);
 	b3LoadSoftBodyUseBendingSprings(command, args.m_useBendingSprings, args.m_springBendingStiffness);

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/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)
@@ -82,7 +82,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;
@@ -91,7 +91,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.));