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

Use two SI solvers as blocks in block solver.

Browse Source 
In the btBlockSolver we are experimenting with, we have SI for both multibody and rigid body. I'm currently replacing rigid body SI solver with two smaller SI solvers. The two examples provided by RigidBodyBoxes.h should have the same behavior.
This commit is contained in:
Chuyuan Fu
2019-03-25 14:41:58 -07:00
parent 19f3ec8b80
commit 7aba1f9e8a
9 changed files with 369 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
examples/BlockSolver/BlockSolverExample.cpp
View File

@@ -13,6 +13,7 @@
class BlockSolverExample : public CommonMultiBodyBase
{
int m_option;
public:
BlockSolverExample(GUIHelperInterface* helper, int option);
virtual ~BlockSolverExample();
@@ -35,10 +36,9 @@ public:
btMultiBody* loadRobot(std::string filepath);
};
BlockSolverExample::BlockSolverExample(GUIHelperInterface* helper, int option)
: CommonMultiBodyBase(helper),
m_option(option)
m_option(option)
{
m_guiHelper->setUpAxis(2);
}
@@ -51,13 +51,12 @@ BlockSolverExample::~BlockSolverExample()
void BlockSolverExample::stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
btScalar internalTimeStep = 1./240.f;
btScalar internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
}
void BlockSolverExample::initPhysics()
{
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btDefaultCollisionConfiguration();
@@ -66,37 +65,34 @@ void BlockSolverExample::initPhysics()
m_broadphase = new btDbvtBroadphase();
btMLCPSolverInterface* mlcp;
if (m_option&BLOCK_SOLVER_SI)
if (m_option & BLOCK_SOLVER_SI)
{
btAssert(!m_solver);
m_solver = new btMultiBodyConstraintSolver;
b3Printf("Constraint Solver: Sequential Impulse");
}
if (m_option&BLOCK_SOLVER_MLCP_PGS)
if (m_option & BLOCK_SOLVER_MLCP_PGS)
{
btAssert(!m_solver);
mlcp = new btSolveProjectedGaussSeidel();
m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + PGS");
}
if (m_option&BLOCK_SOLVER_MLCP_DANTZIG)
if (m_option & BLOCK_SOLVER_MLCP_DANTZIG)
{
btAssert(!m_solver);
mlcp = new btDantzigSolver();
m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + Dantzig");
}
if (m_option&BLOCK_SOLVER_BLOCK)
if (m_option & BLOCK_SOLVER_BLOCK)
{
m_solver = new btBlockSolver();
}
btAssert(m_solver);
btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_dynamicsWorld = world;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
@@ -104,16 +100,14 @@ void BlockSolverExample::initPhysics()
m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-6); //todo: what value is good?
if (m_option&BLOCK_SOLVER_SCENE_MB_STACK)
if (m_option & BLOCK_SOLVER_SCENE_MB_STACK)
{
createMultiBodyStack();
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void BlockSolverExample::createMultiBodyStack()
{
///create a few basic rigid bodies
@@ -134,7 +128,7 @@ void BlockSolverExample::createMultiBodyStack()
btMultiBody* body = createMultiBody(mass, tr, groundShape);
}
for (int i=0;i<10;i++)
for (int i = 0; i < 10; i++)
{
btBoxShape* boxShape = createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1)));
m_collisionShapes.push_back(boxShape);
@@ -143,10 +137,10 @@ void BlockSolverExample::createMultiBodyStack()
mass = 100;
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0, 0, 0.1+i*0.2));
tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2));
btMultiBody* body = createMultiBody(mass, tr, boxShape);
}
if(0)
if (0)
{
btMultiBody* mb = loadRobot("cube_small.urdf");
btTransform tr;
@@ -173,25 +167,21 @@ btMultiBody* BlockSolverExample::createMultiBody(btScalar mass, const btTransfor
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
this->m_dynamicsWorld->addCollisionObject(collider, collisionFilterGroup, collisionFilterMask);
mb->setBaseCollider(collider);
mb->finalizeMultiDof();
this->m_dynamicsWorld->addMultiBody(mb);
m_dynamicsWorld->forwardKinematics();
return mb;
}
btMultiBody* BlockSolverExample::loadRobot(std::string filepath)
{
btMultiBody* m_multiBody = 0;
BulletURDFImporter u2b(m_guiHelper, 0, 0, 1, 0);
bool loadOk = u2b.loadURDF(filepath.c_str());// lwr / kuka.urdf");
bool loadOk = u2b.loadURDF(filepath.c_str()); // lwr / kuka.urdf");
if (loadOk)
{
int rootLinkIndex = u2b.getRootLinkIndex();

12
examples/BlockSolver/BlockSolverExample.h
View File

@@ -2,18 +2,6 @@
#ifndef BLOCK_SOLVER_EXAMPLE_H
#define BLOCK_SOLVER_EXAMPLE_H
enum BlockSolverOptions
{
BLOCK_SOLVER_SI=1<<0,
BLOCK_SOLVER_MLCP_PGS = 1 << 1,
BLOCK_SOLVER_MLCP_DANTZIG = 1 << 2,
BLOCK_SOLVER_BLOCK = 1 << 3,
BLOCK_SOLVER_SCENE_MB_STACK= 1 << 5,
BLOCK_SOLVER_SCENE_CHAIN = 1<< 6,
};
class CommonExampleInterface* BlockSolverExampleCreateFunc(struct CommonExampleOptions& options);
#endif //BLOCK_SOLVER_EXAMPLE_H

334
examples/BlockSolver/btBlockSolver.cpp
View File

@@ -1,7 +1,17 @@
#include "btBlockSolver.h"
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "LinearMath/btQuickprof.h"
void setupHelper(btSISolverSingleIterationData& siData,
btCollisionObject** bodies, int numBodies,
const btContactSolverInfo& info,
btTypedConstraint** constraintStart, int constrainNums,
btPersistentManifold** manifoldPtr, int numManifolds);
struct btBlockSolverInternalData
{
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
@@ -13,86 +23,273 @@ struct btBlockSolverInternalData
btAlignedObjectArray<int> m_orderTmpConstraintPool;
btAlignedObjectArray<int> m_orderNonContactConstraintPool;
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>
m_tmpConstraintSizesPool;
unsigned long m_btSeed2;
int m_fixedBodyId;
int m_maxOverrideNumSolverIterations;
btAlignedObjectArray<int> m_kinematicBodyUniqueIdToSolverBodyTable; // only used for multithreading
btAlignedObjectArray<int>
m_kinematicBodyUniqueIdToSolverBodyTable; // only used for multithreading
btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;
btBlockSolverInternalData()
:m_btSeed2(0),
m_fixedBodyId(-1),
m_maxOverrideNumSolverIterations(0),
m_resolveSingleConstraintRowGeneric(btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()),
m_resolveSingleConstraintRowLowerLimit(btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()),
m_resolveSplitPenetrationImpulse(btSequentialImpulseConstraintSolver::getScalarSplitPenetrationImpulseGeneric())
{
}
: m_btSeed2(0),
m_fixedBodyId(-1),
m_maxOverrideNumSolverIterations(0),
m_resolveSingleConstraintRowGeneric(
btSequentialImpulseConstraintSolver::
getScalarConstraintRowSolverGeneric()),
m_resolveSingleConstraintRowLowerLimit(
btSequentialImpulseConstraintSolver::
getScalarConstraintRowSolverLowerLimit()),
m_resolveSplitPenetrationImpulse(
btSequentialImpulseConstraintSolver::
getScalarSplitPenetrationImpulseGeneric()) {}
};
btBlockSolver::btBlockSolver()
{
m_data2 = new btBlockSolverInternalData;
m_data21 = new btBlockSolverInternalData;
m_data22 = new btBlockSolverInternalData;
}
btBlockSolver::~btBlockSolver()
{
delete m_data2;
delete m_data21;
delete m_data22;
}
btScalar btBlockSolver::solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
btScalar btBlockSolver::solveGroupInternalBlock(
btCollisionObject** bodies, int numBodies,
btPersistentManifold** manifoldPtr, int numManifolds,
btTypedConstraint** constraints, int numConstraints,
const btContactSolverInfo& info, btIDebugDraw* debugDrawer,
btDispatcher* dispatcher)
{
// initialize data for two children solvers
btSISolverSingleIterationData siData1(
m_data21->m_tmpSolverBodyPool, m_data21->m_tmpSolverContactConstraintPool,
m_data21->m_tmpSolverNonContactConstraintPool,
m_data21->m_tmpSolverContactFrictionConstraintPool,
m_data21->m_tmpSolverContactRollingFrictionConstraintPool,
m_data21->m_orderTmpConstraintPool,
m_data21->m_orderNonContactConstraintPool,
m_data21->m_orderFrictionConstraintPool,
m_data21->m_tmpConstraintSizesPool,
m_data21->m_resolveSingleConstraintRowGeneric,
m_data21->m_resolveSingleConstraintRowLowerLimit,
m_data21->m_resolveSplitPenetrationImpulse,
m_data21->m_kinematicBodyUniqueIdToSolverBodyTable, m_data21->m_btSeed2,
m_data21->m_fixedBodyId, m_data21->m_maxOverrideNumSolverIterations);
btSISolverSingleIterationData siData(m_data2->m_tmpSolverBodyPool,
m_data2->m_tmpSolverContactConstraintPool,
m_data2->m_tmpSolverNonContactConstraintPool,
m_data2->m_tmpSolverContactFrictionConstraintPool,
m_data2->m_tmpSolverContactRollingFrictionConstraintPool,
m_data2->m_orderTmpConstraintPool,
m_data2->m_orderNonContactConstraintPool,
m_data2->m_orderFrictionConstraintPool,
m_data2->m_tmpConstraintSizesPool,
m_data2->m_resolveSingleConstraintRowGeneric,
m_data2->m_resolveSingleConstraintRowLowerLimit,
m_data2->m_resolveSplitPenetrationImpulse,
m_data2->m_kinematicBodyUniqueIdToSolverBodyTable,
m_data2->m_btSeed2,
m_data2->m_fixedBodyId,
m_data2->m_maxOverrideNumSolverIterations);
m_data2->m_fixedBodyId = -1;
//todo: setup sse2/4 constraint row methods
btSISolverSingleIterationData siData2(
m_data22->m_tmpSolverBodyPool, m_data22->m_tmpSolverContactConstraintPool,
m_data22->m_tmpSolverNonContactConstraintPool,
m_data22->m_tmpSolverContactFrictionConstraintPool,
m_data22->m_tmpSolverContactRollingFrictionConstraintPool,
m_data22->m_orderTmpConstraintPool,
m_data22->m_orderNonContactConstraintPool,
m_data22->m_orderFrictionConstraintPool,
m_data22->m_tmpConstraintSizesPool,
m_data22->m_resolveSingleConstraintRowGeneric,
m_data22->m_resolveSingleConstraintRowLowerLimit,
m_data22->m_resolveSplitPenetrationImpulse,
m_data22->m_kinematicBodyUniqueIdToSolverBodyTable, m_data22->m_btSeed2,
m_data22->m_fixedBodyId, m_data22->m_maxOverrideNumSolverIterations);
m_data21->m_fixedBodyId = -1;
m_data22->m_fixedBodyId = -1;
// set up
int halfNumConstraints1 = numConstraints / 2;
int halfNumConstraints2 = numConstraints - halfNumConstraints1;
int halfNumManifolds1 = numConstraints / 2;
int halfNumManifolds2 = numManifolds - halfNumManifolds1;
setupHelper(siData1, bodies, numBodies, info, constraints,
halfNumConstraints1, manifoldPtr, halfNumManifolds1);
setupHelper(siData2, bodies, numBodies, info,
constraints + halfNumConstraints1, halfNumConstraints2,
manifoldPtr + halfNumManifolds1, halfNumManifolds2);
// set up complete
// begin solve
btScalar leastSquaresResidual = 0;
{
BT_PROFILE("solveGroupCacheFriendlyIterations");
/// this is a special step to resolve penetrations (just for contacts)
btSequentialImpulseConstraintSolver::
solveGroupCacheFriendlySplitImpulseIterationsInternal(
siData1, bodies, numBodies, manifoldPtr, halfNumManifolds1,
constraints, halfNumConstraints1, info, debugDrawer);
btSequentialImpulseConstraintSolver::
solveGroupCacheFriendlySplitImpulseIterationsInternal(
siData2, bodies, numBodies, manifoldPtr + halfNumManifolds1,
halfNumManifolds2, constraints + halfNumConstraints1,
halfNumConstraints2, info, debugDrawer);
int maxIterations =
siData1.m_maxOverrideNumSolverIterations > info.m_numIterations
? siData1.m_maxOverrideNumSolverIterations
: info.m_numIterations;
for (int iteration = 0; iteration < maxIterations; iteration++)
{
auto res1 =
btSequentialImpulseConstraintSolver::solveSingleIterationInternal(
siData1, iteration, constraints, halfNumConstraints1, info);
auto res2 =
btSequentialImpulseConstraintSolver::solveSingleIterationInternal(
siData2, iteration, constraints + halfNumConstraints1,
halfNumConstraints2, info);
leastSquaresResidual = btMax(res1, res2);
if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold ||
(iteration >= (maxIterations - 1)))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual,
iteration);
#endif
break;
}
}
}
btScalar res = btSequentialImpulseConstraintSolver::
solveGroupCacheFriendlyFinishInternal(siData1, bodies, numBodies, info);
+btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(
siData2, bodies, numBodies, info);
return res;
}
void setupHelper(btSISolverSingleIterationData& siData,
btCollisionObject** bodies, int numBodies,
const btContactSolverInfo& info,
btTypedConstraint** constraintStart, int constrainNums,
btPersistentManifold** manifoldPtr, int numManifolds)
{
btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies,
numBodies, info);
btSequentialImpulseConstraintSolver::convertJointsInternal(
siData, constraintStart, constrainNums, info);
btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies, numBodies, info);
btSequentialImpulseConstraintSolver::convertJointsInternal(siData, constraints, numConstraints, info);
int i;
btPersistentManifold* manifold = 0;
// btCollisionObject* colObj0=0,*colObj1=0;
for (i = 0; i < numManifolds; i++)
{
manifold = manifoldPtr[i];
btSequentialImpulseConstraintSolver::convertContactInternal(siData, manifold, info);
btSequentialImpulseConstraintSolver::convertContactInternal(siData,
manifold, info);
int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size();
int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size();
int numFrictionPool =
siData.m_tmpSolverContactFrictionConstraintPool.size();
siData.m_orderNonContactConstraintPool.resizeNoInitialize(
numNonContactPool);
if ((info.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
else
siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
siData.m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
{
int i;
for (i = 0; i < numNonContactPool; i++)
{
siData.m_orderNonContactConstraintPool[i] = i;
}
for (i = 0; i < numConstraintPool; i++)
{
siData.m_orderTmpConstraintPool[i] = i;
}
for (i = 0; i < numFrictionPool; i++)
{
siData.m_orderFrictionConstraintPool[i] = i;
}
}
}
}
btScalar btBlockSolver::solveGroup(btCollisionObject** bodies, int numBodies,
btPersistentManifold** manifoldPtr,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& info,
btIDebugDraw* debugDrawer,
btDispatcher* dispatcher)
{
// if (m_childSolvers.size())
// hard code to use block solver for now
return solveGroupInternalBlock(bodies, numBodies, manifoldPtr, numManifolds,
constraints, numConstraints, info, debugDrawer,
dispatcher);
// else
// return solveGroupInternal(bodies, numBodies, manifoldPtr, numManifolds,
// constraints, numConstraints, info, debugDrawer,
// dispatcher);
}
btScalar btBlockSolver::solveGroupInternal(
btCollisionObject** bodies, int numBodies,
btPersistentManifold** manifoldPtr, int numManifolds,
btTypedConstraint** constraints, int numConstraints,
const btContactSolverInfo& info, btIDebugDraw* debugDrawer,
btDispatcher* dispatcher)
{
btSISolverSingleIterationData siData(
m_data21->m_tmpSolverBodyPool, m_data21->m_tmpSolverContactConstraintPool,
m_data21->m_tmpSolverNonContactConstraintPool,
m_data21->m_tmpSolverContactFrictionConstraintPool,
m_data21->m_tmpSolverContactRollingFrictionConstraintPool,
m_data21->m_orderTmpConstraintPool,
m_data21->m_orderNonContactConstraintPool,
m_data21->m_orderFrictionConstraintPool,
m_data21->m_tmpConstraintSizesPool,
m_data21->m_resolveSingleConstraintRowGeneric,
m_data21->m_resolveSingleConstraintRowLowerLimit,
m_data21->m_resolveSplitPenetrationImpulse,
m_data21->m_kinematicBodyUniqueIdToSolverBodyTable, m_data21->m_btSeed2,
m_data21->m_fixedBodyId, m_data21->m_maxOverrideNumSolverIterations);
m_data21->m_fixedBodyId = -1;
// todo: setup sse2/4 constraint row methods
btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies,
numBodies, info);
btSequentialImpulseConstraintSolver::convertJointsInternal(
siData, constraints, numConstraints, info);
int i;
btPersistentManifold* manifold = 0;
// btCollisionObject* colObj0=0,*colObj1=0;
for (i = 0; i < numManifolds; i++)
{
manifold = manifoldPtr[i];
btSequentialImpulseConstraintSolver::convertContactInternal(siData,
manifold, info);
}
int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size();
int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size();
int numFrictionPool = siData.m_tmpSolverContactFrictionConstraintPool.size();
///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
// @todo: use stack allocator for such temporarily memory, same for solver
// bodies/constraints
siData.m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
if ((info.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
@@ -118,43 +315,60 @@ btScalar btBlockSolver::solveGroup(btCollisionObject * *bodies, int numBodies, b
btScalar leastSquaresResidual = 0;
{
BT_PROFILE("solveGroupCacheFriendlyIterations");
///this is a special step to resolve penetrations (just for contacts)
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterationsInternal(siData, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, info, debugDrawer);
/// this is a special step to resolve penetrations (just for contacts)
btSequentialImpulseConstraintSolver::
solveGroupCacheFriendlySplitImpulseIterationsInternal(
siData, bodies, numBodies, manifoldPtr, numManifolds, constraints,
numConstraints, info, debugDrawer);
int maxIterations = siData.m_maxOverrideNumSolverIterations > info.m_numIterations ? siData.m_maxOverrideNumSolverIterations : info.m_numIterations;
int maxIterations =
siData.m_maxOverrideNumSolverIterations > info.m_numIterations
? siData.m_maxOverrideNumSolverIterations
: info.m_numIterations;
for (int iteration = 0; iteration < maxIterations; iteration++)
{
leastSquaresResidual = btSequentialImpulseConstraintSolver::solveSingleIterationInternal(siData, iteration, constraints, numConstraints, info);
leastSquaresResidual =
btSequentialImpulseConstraintSolver::solveSingleIterationInternal(
siData, iteration, constraints, numConstraints, info);
if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold ||
(iteration >= (maxIterations - 1)))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual,
iteration);
#endif
break;
}
}
}
btScalar res = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, info);
btScalar res = btSequentialImpulseConstraintSolver::
solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, info);
return res;
}
void btBlockSolver::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 btBlockSolver::solveMultiBodyGroup(
btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold,
int numManifolds, btTypedConstraint** constraints, int numConstraints,
btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints,
const btContactSolverInfo& info, btIDebugDraw* debugDrawer,
btDispatcher* dispatcher)
{
btMultiBodyConstraintSolver::solveMultiBodyGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, multiBodyConstraints, numMultiBodyConstraints, info, debugDrawer, dispatcher);
btMultiBodyConstraintSolver::solveMultiBodyGroup(
bodies, numBodies, manifold, numManifolds, constraints, numConstraints,
multiBodyConstraints, numMultiBodyConstraints, info, debugDrawer,
dispatcher);
}
void btBlockSolver::reset()
{
//or just set m_data2->m_btSeed2=0?
delete m_data2;
m_data2 = new btBlockSolverInternalData;
// or just set m_data2->m_btSeed2=0?
delete m_data21;
delete m_data22;
m_data21 = new btBlockSolverInternalData;
m_data22 = new btBlockSolverInternalData;
}

70
examples/BlockSolver/btBlockSolver.h
View File

@@ -1,24 +1,72 @@
#ifndef BT_BLOCK_SOLVER_H
#define BT_BLOCK_SOLVER_H
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "Bullet3Common/b3Logging.h"
enum BlockSolverOptions
{
BLOCK_SOLVER_SI = 1 << 0,
BLOCK_SOLVER_MLCP_PGS = 1 << 1,
BLOCK_SOLVER_MLCP_DANTZIG = 1 << 2,
BLOCK_SOLVER_BLOCK = 1 << 3,
BLOCK_SOLVER_SCENE_MB_STACK = 1 << 5,
BLOCK_SOLVER_SCENE_CHAIN = 1 << 6,
};
class btBlockSolver : public btMultiBodyConstraintSolver
{
struct btBlockSolverInternalData* m_data2;
struct btBlockSolverInternalData* m_data21;
struct btBlockSolverInternalData* m_data22;
public
: btBlockSolver();
public:
btBlockSolver();
virtual ~btBlockSolver();
virtual ~btBlockSolver();
//btRigidBody
virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, class btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
// btRigidBody
virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies,
btPersistentManifold** manifoldPtr,
int numManifolds, btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& info,
class btIDebugDraw* debugDrawer,
btDispatcher* dispatcher);
//btMultibody
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);
btScalar solveGroupInternal(btCollisionObject** bodies, int numBodies,
btPersistentManifold** manifoldPtr,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& info,
btIDebugDraw* debugDrawer,
btDispatcher* dispatcher);
///clear internal cached data and reset random seed
virtual void reset();
btScalar solveGroupInternalBlock(btCollisionObject** bodies, int numBodies,
btPersistentManifold** manifoldPtr,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& info,
btIDebugDraw* debugDrawer,
btDispatcher* dispatcher);
// btMultibody
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);
/// clear internal cached data and reset random seed
virtual void reset();
virtual btConstraintSolverType getSolverType() const
{
@@ -26,4 +74,4 @@ public:
}
};
#endif //BT_BLOCK_SOLVER_H
#endif //BT_BLOCK_SOLVER_H