From f65a8b03c00447d882e132f45fbfd4b4d68b81c8 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 17 Dec 2019 18:27:23 -0800 Subject: [PATCH] separate deformable contact solve by islands WIP --- .../b3RobotSimulatorClientAPI_NoDirect.cpp | 2 +- .../btMultiBodyInplaceSolverIslandCallback.h | 6 +++ ...eformableBodyInplaceSolverIslandCallback.h | 46 +++++++++++++++++++ .../btDeformableMultiBodyConstraintSolver.cpp | 6 +-- .../btDeformableMultiBodyConstraintSolver.h | 5 +- .../btDeformableMultiBodyDynamicsWorld.cpp | 33 +++++++++++-- .../btDeformableMultiBodyDynamicsWorld.h | 27 ++--------- 7 files changed, 92 insertions(+), 33 deletions(-) create mode 100644 src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index d8cd3d495..68bf2d231 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/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); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h b/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h index 593f68d9a..3169b86e6 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h @@ -76,6 +76,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: btDispatcher* m_dispatcher; btAlignedObjectArray m_bodies; + btAlignedObjectArray m_softBodies; btAlignedObjectArray m_manifolds; btAlignedObjectArray m_constraints; btAlignedObjectArray 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); diff --git a/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h b/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h new file mode 100644 index 000000000..7b225701f --- /dev/null +++ b/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 */ diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 581d6feae..d2c810d0b 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -17,7 +17,7 @@ #include "btDeformableMultiBodyConstraintSolver.h" #include // 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); diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h index 10f1d3f4b..0c7cc26a8 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h +++ b/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 */ diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 25ee65450..618e5c0d7 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -36,9 +36,36 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed #include #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 diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 9fbebd6ed..763038576 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -29,6 +29,7 @@ typedef btAlignedObjectArray btSoftBodyArray; class btDeformableBodySolver; class btDeformableLagrangianForce; struct MultiBodyInplaceSolverIslandCallback; +struct DeformableBodyInplaceSolverIslandCallback; class btDeformableMultiBodyConstraintSolver; typedef btAlignedObjectArray 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) - : 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; - } + btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0); virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));