Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

separate deformable contact solve by islands WIP

Browse Source
This commit is contained in:
Xuchen Han
2019-12-17 18:27:23 -08:00
parent d38ea87027
commit f65a8b03c0
7 changed files with 92 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -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);

6
src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h
View File

@@ -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);

46
src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h Normal file
View File

@@ -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 */

6
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@@ -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);

5
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h
View File

@@ -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 */

33
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@@ -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

27
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
View File

@@ -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)
: 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.));