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

Merge pull request #2407 from erwincoumans/master

Browse Source 
fix multithreaded solver
This commit is contained in:
erwincoumans
2019-09-13 07:39:05 -07:00
committed by GitHub
parent 66fc3a9ce9 95a7488310
commit 8ad4afad70
13 changed files with 172 additions and 1643 deletions
Download Patch File Download Diff File Expand all files Collapse all files

209
examples/BlockSolver/BlockSolverExample.cpp
View File

@@ -1,209 +0,0 @@
#include "BlockSolverExample.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "btBlockSolver.h"
//for URDF import support
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
class BlockSolverExample : public CommonMultiBodyBase
{
int m_option;
public:
BlockSolverExample(GUIHelperInterface* helper, int option);
virtual ~BlockSolverExample();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 3;
float pitch = -35;
float yaw = 50;
float targetPos[3] = {0, 0, .1};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void createMultiBodyStack();
btMultiBody* createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape);
btMultiBody* loadRobot(std::string filepath);
};
BlockSolverExample::BlockSolverExample(GUIHelperInterface* helper, int option)
: CommonMultiBodyBase(helper),
m_option(option)
{
m_guiHelper->setUpAxis(2);
}
BlockSolverExample::~BlockSolverExample()
{
// Do nothing
}
void BlockSolverExample::stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
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();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btMLCPSolverInterface* mlcp;
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)
{
btAssert(!m_solver);
mlcp = new btSolveProjectedGaussSeidel();
m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + PGS");
}
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)
{
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);
m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
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)
{
createMultiBodyStack();
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void BlockSolverExample::createMultiBodyStack()
{
///create a few basic rigid bodies
bool loadPlaneFromURDF = false;
if (loadPlaneFromURDF)
{
btMultiBody* mb = loadRobot("plane.urdf");
printf("!\n");
}
else
{
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.), btScalar(50.), btScalar(50.)));
m_collisionShapes.push_back(groundShape);
btScalar mass = 0;
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0, 0, -50));
btMultiBody* body = createMultiBody(mass, tr, groundShape);
}
for (int i = 0; i < 10; i++)
{
btBoxShape* boxShape = createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1)));
m_collisionShapes.push_back(boxShape);
btScalar mass = 1;
if (i == 9)
mass = 100;
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2));
btMultiBody* body = createMultiBody(mass, tr, boxShape);
}
if (/* DISABLES CODE */ (0))
{
btMultiBody* mb = loadRobot("cube_small.urdf");
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0, 0, 1.));
mb->setBaseWorldTransform(tr);
}
}
btMultiBody* BlockSolverExample::createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape)
{
btVector3 inertia;
collisionShape->calculateLocalInertia(mass, inertia);
bool canSleep = false;
bool isDynamic = mass > 0;
btMultiBody* mb = new btMultiBody(0, mass, inertia, !isDynamic, canSleep);
btMultiBodyLinkCollider* collider = new btMultiBodyLinkCollider(mb, -1);
collider->setWorldTransform(trans);
mb->setBaseWorldTransform(trans);
collider->setCollisionShape(collisionShape);
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");
if (loadOk)
{
int rootLinkIndex = u2b.getRootLinkIndex();
b3Printf("urdf root link index = %d\n", rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
btTransform identityTrans;
identityTrans.setIdentity();
ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, true, u2b.getPathPrefix());
for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
{
m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
}
m_multiBody = creation.getBulletMultiBody();
if (m_multiBody)
{
b3Printf("Root link name = %s", u2b.getLinkName(u2b.getRootLinkIndex()).c_str());
}
}
return m_multiBody;
}
CommonExampleInterface* BlockSolverExampleCreateFunc(CommonExampleOptions& options)
{
return new BlockSolverExample(options.m_guiHelper, options.m_option);
}

7
examples/BlockSolver/BlockSolverExample.h
View File

@@ -1,7 +0,0 @@
#ifndef BLOCK_SOLVER_EXAMPLE_H
#define BLOCK_SOLVER_EXAMPLE_H
class CommonExampleInterface* BlockSolverExampleCreateFunc(struct CommonExampleOptions& options);
#endif //BLOCK_SOLVER_EXAMPLE_H

151
examples/BlockSolver/RigidBodyBoxes.cpp
View File

@@ -1,151 +0,0 @@
#include "RigidBodyBoxes.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "BlockSolverExample.h"
#include "btBlockSolver.h"
class RigidBodyBoxes : public CommonRigidBodyBase
{
int m_option;
int m_numIterations;
int m_numBoxes;
btAlignedObjectArray<btRigidBody*> boxes;
static btScalar numSolverIterations;
public:
RigidBodyBoxes(GUIHelperInterface* helper, int option);
virtual ~RigidBodyBoxes();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
void resetCubePosition();
virtual void resetCamera()
{
float dist = 3;
float pitch = -35;
float yaw = 50;
float targetPos[3] = {0, 0, .1};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1],
targetPos[2]);
}
void createRigidBodyStack();
};
btScalar RigidBodyBoxes::numSolverIterations = 50;
RigidBodyBoxes::RigidBodyBoxes(GUIHelperInterface* helper, int option)
: CommonRigidBodyBase(helper),
m_option(option),
m_numIterations(numSolverIterations),
m_numBoxes(4)
{
m_guiHelper->setUpAxis(2);
}
RigidBodyBoxes::~RigidBodyBoxes()
{
// Do nothing
}
void RigidBodyBoxes::createRigidBodyStack()
{
// create ground
btBoxShape* groundShape =
createBoxShape(btVector3(btScalar(5.), btScalar(5.), btScalar(5.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, 0, -5));
btScalar mass(0.);
btRigidBody* body = createRigidBody(mass, groundTransform, groundShape,
btVector4(0, 0, 1, 1));
// create a few boxes
mass = 1;
for (int i = 0; i < m_numBoxes; i++)
{
btBoxShape* boxShape =
createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1)));
m_collisionShapes.push_back(boxShape);
mass *= 4;
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2));
boxes.push_back(createRigidBody(mass, tr, boxShape));
}
}
void RigidBodyBoxes::initPhysics()
{
/// collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btDefaultCollisionConfiguration();
/// use the default collision dispatcher. For parallel processing you can use
/// a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
{
SliderParams slider("numSolverIterations", &numSolverIterations);
slider.m_minVal = 5;
slider.m_maxVal = 500;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
if (m_option & BLOCK_SOLVER_SI)
{
m_solver = new btSequentialImpulseConstraintSolver;
b3Printf("Constraint Solver: Sequential Impulse");
}
if (m_option & BLOCK_SOLVER_BLOCK)
{
m_solver = new btBlockSolver();
b3Printf("Constraint Solver: Block solver");
}
btAssert(m_solver);
m_dynamicsWorld = new btDiscreteDynamicsWorld(
m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
createRigidBodyStack();
m_dynamicsWorld->getSolverInfo().m_numIterations = numSolverIterations;
m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-6);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void RigidBodyBoxes::resetCubePosition()
{
for (int i = 0; i < m_numBoxes; i++)
{
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2));
boxes[i]->setWorldTransform(tr);
}
}
void RigidBodyBoxes::stepSimulation(float deltaTime)
{
if ((int)numSolverIterations != m_numIterations)
{
resetCubePosition();
m_numIterations = (int)numSolverIterations;
m_dynamicsWorld->getSolverInfo().m_numIterations = m_numIterations;
b3Printf("New num iterations; %d", m_numIterations);
}
m_dynamicsWorld->stepSimulation(deltaTime);
}
CommonExampleInterface* RigidBodyBoxesCreateFunc(
CommonExampleOptions& options)
{
return new RigidBodyBoxes(options.m_guiHelper, options.m_option);
}

6
examples/BlockSolver/RigidBodyBoxes.h
View File

@@ -1,6 +0,0 @@
#ifndef BLOCKSOLVER_RIGIDBODYBOXES_H_
#define BLOCKSOLVER_RIGIDBODYBOXES_H_
class CommonExampleInterface* RigidBodyBoxesCreateFunc(struct CommonExampleOptions& options);
#endif //BLOCKSOLVER_RIGIDBODYBOXES_H_

374
examples/BlockSolver/btBlockSolver.cpp
View File

@@ -1,374 +0,0 @@
#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;
btConstraintArray m_tmpSolverContactConstraintPool;
btConstraintArray m_tmpSolverNonContactConstraintPool;
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
btAlignedObjectArray<int> m_orderTmpConstraintPool;
btAlignedObjectArray<int> m_orderNonContactConstraintPool;
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>
m_tmpConstraintSizesPool;
unsigned long m_btSeed2;
int m_fixedBodyId;
int m_maxOverrideNumSolverIterations;
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()) {}
};
btBlockSolver::btBlockSolver()
{
m_data21 = new btBlockSolverInternalData;
m_data22 = new btBlockSolverInternalData;
}
btBlockSolver::~btBlockSolver()
{
delete m_data21;
delete m_data22;
}
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 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++)
{
btScalar res1 =
btSequentialImpulseConstraintSolver::solveSingleIterationInternal(
siData1, iteration, constraints, halfNumConstraints1, info);
btScalar 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);
int i;
btPersistentManifold* manifold = 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();
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
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 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);
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);
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(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)
{
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_data21;
delete m_data22;
m_data21 = new btBlockSolverInternalData;
m_data22 = new btBlockSolverInternalData;
}

77
examples/BlockSolver/btBlockSolver.h
View File

@@ -1,77 +0,0 @@
#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_data21;
struct btBlockSolverInternalData* m_data22;
public
: 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);
btScalar solveGroupInternal(btCollisionObject** bodies, int numBodies,
btPersistentManifold** manifoldPtr,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& info,
btIDebugDraw* debugDrawer,
btDispatcher* dispatcher);
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
{
return BT_BLOCK_SOLVER;
}
};
#endif //BT_BLOCK_SOLVER_H

6
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -220,12 +220,6 @@ SET(BulletExampleBrowser_SRCS
../MultiThreadedDemo/CommonRigidBodyMTBase.h
../Heightfield/HeightfieldExample.cpp
../Heightfield/HeightfieldExample.h
../BlockSolver/btBlockSolver.cpp
../BlockSolver/btBlockSolver.h
../BlockSolver/BlockSolverExample.cpp
../BlockSolver/BlockSolverExample.h
../BlockSolver/RigidBodyBoxes.cpp
../BlockSolver/RigidBodyBoxes.h
../Tutorial/Tutorial.cpp
../Tutorial/Tutorial.h
../Tutorial/Dof6ConstraintTutorial.cpp

10
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -1,8 +1,5 @@
#include "ExampleEntries.h"
#include "../BlockSolver/btBlockSolver.h"
#include "../BlockSolver/BlockSolverExample.h"
#include "../BlockSolver/RigidBodyBoxes.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "EmptyExample.h"
#include "../Heightfield/HeightfieldExample.h"
@@ -164,13 +161,6 @@ static ExampleEntry gDefaultExamples[] =
//
// ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
ExampleEntry(0, "BlockSolver"),
ExampleEntry(1, "Stack MultiBody SI", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_SI),
ExampleEntry(1, "Stack MultiBody MLCP PGS", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_PGS),
ExampleEntry(1, "Stack MultiBody MLCP Dantzig", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_DANTZIG),
ExampleEntry(1, "Stack MultiBody Block", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_BLOCK),
//ExampleEntry(1, "Stack RigidBody SI", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_SI),
//ExampleEntry(1, "Stack RigidBody Block", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_BLOCK),
ExampleEntry(0, "Inverse Dynamics"),
ExampleEntry(1, "Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc, BT_ID_LOAD_URDF),

1
examples/ExampleBrowser/premake4.lua
View File

@@ -168,7 +168,6 @@ project "App_BulletExampleBrowser"
"../Evolution/NN3DWalkers.h",
"../Collision/*",
"../RoboticsLearning/*",
"../BlockSolver/*",
"../Collision/Internal/*",
"../Benchmarks/*",
"../MultiThreadedDemo/*",

4
examples/pybullet/examples/createMultiBodyBatch.py
View File

@@ -117,14 +117,14 @@ for x in range(32):
batchPositions.append(
[x * meshScale[0] * 5.5, y * meshScale[1] * 5.5, (0.5 + z) * meshScale[2] * 2.5])
bodyUid = p.createMultiBody(baseMass=0,
bodyUids = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0, 0, 2],
batchPositions=batchPositions,
useMaximalCoordinates=True)
p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
p.changeVisualShape(bodyUids[0], -1, textureUniqueId=texUid)
p.syncBodyInfo()
print("numBodies=", p.getNumBodies())

825
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

File diff suppressed because it is too large Load Diff

124
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -29,68 +29,6 @@ class btCollisionObject;
typedef btScalar (*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
struct btSISolverSingleIterationData
{
btAlignedObjectArray<btSolverBody>& m_tmpSolverBodyPool;
btConstraintArray& m_tmpSolverContactConstraintPool;
btConstraintArray& m_tmpSolverNonContactConstraintPool;
btConstraintArray& m_tmpSolverContactFrictionConstraintPool;
btConstraintArray& m_tmpSolverContactRollingFrictionConstraintPool;
btAlignedObjectArray<int>& m_orderTmpConstraintPool;
btAlignedObjectArray<int>& m_orderNonContactConstraintPool;
btAlignedObjectArray<int>& m_orderFrictionConstraintPool;
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>& m_tmpConstraintSizesPool;
unsigned long& m_seed;
btSingleConstraintRowSolver& m_resolveSingleConstraintRowGeneric;
btSingleConstraintRowSolver& m_resolveSingleConstraintRowLowerLimit;
btSingleConstraintRowSolver& m_resolveSplitPenetrationImpulse;
btAlignedObjectArray<int>& m_kinematicBodyUniqueIdToSolverBodyTable;
int& m_fixedBodyId;
int& m_maxOverrideNumSolverIterations;
int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep);
static void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep);
int getSolverBody(btCollisionObject& body) const;
btSISolverSingleIterationData(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool,
btConstraintArray& tmpSolverContactConstraintPool,
btConstraintArray& tmpSolverNonContactConstraintPool,
btConstraintArray& tmpSolverContactFrictionConstraintPool,
btConstraintArray& tmpSolverContactRollingFrictionConstraintPool,
btAlignedObjectArray<int>& orderTmpConstraintPool,
btAlignedObjectArray<int>& orderNonContactConstraintPool,
btAlignedObjectArray<int>& orderFrictionConstraintPool,
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>& tmpConstraintSizesPool,
btSingleConstraintRowSolver& resolveSingleConstraintRowGeneric,
btSingleConstraintRowSolver& resolveSingleConstraintRowLowerLimit,
btSingleConstraintRowSolver& resolveSplitPenetrationImpulse,
btAlignedObjectArray<int>& kinematicBodyUniqueIdToSolverBodyTable,
unsigned long& seed,
int& fixedBodyId,
int& maxOverrideNumSolverIterations
)
:m_tmpSolverBodyPool(tmpSolverBodyPool),
m_tmpSolverContactConstraintPool(tmpSolverContactConstraintPool),
m_tmpSolverNonContactConstraintPool(tmpSolverNonContactConstraintPool),
m_tmpSolverContactFrictionConstraintPool(tmpSolverContactFrictionConstraintPool),
m_tmpSolverContactRollingFrictionConstraintPool(tmpSolverContactRollingFrictionConstraintPool),
m_orderTmpConstraintPool(orderTmpConstraintPool),
m_orderNonContactConstraintPool(orderNonContactConstraintPool),
m_orderFrictionConstraintPool(orderFrictionConstraintPool),
m_tmpConstraintSizesPool(tmpConstraintSizesPool),
m_seed(seed),
m_resolveSingleConstraintRowGeneric(resolveSingleConstraintRowGeneric),
m_resolveSingleConstraintRowLowerLimit(resolveSingleConstraintRowLowerLimit),
m_resolveSplitPenetrationImpulse(resolveSplitPenetrationImpulse),
m_kinematicBodyUniqueIdToSolverBodyTable(kinematicBodyUniqueIdToSolverBodyTable),
m_fixedBodyId(fixedBodyId),
m_maxOverrideNumSolverIterations(maxOverrideNumSolverIterations)
{
}
};
struct btSolverAnalyticsData
{
btSolverAnalyticsData()
@@ -178,7 +116,6 @@ protected:
virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
void convertJoint(btSolverConstraint * currentConstraintRow, btTypedConstraint * constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
@@ -204,8 +141,7 @@ protected:
return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
}
public:
protected:
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
@@ -213,7 +149,6 @@ public:
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlySetup(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);
@@ -225,51 +160,12 @@ public:
virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
static btScalar solveSingleIterationInternal(btSISolverSingleIterationData& siData, int iteration, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
static void convertBodiesInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
static void convertJointsInternal(btSISolverSingleIterationData& siData, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
static void convertContactInternal(btSISolverSingleIterationData& siData, btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
static void setupContactConstraintInternal(btSISolverSingleIterationData& siData, btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation,
const btVector3& rel_pos1, const btVector3& rel_pos2);
static btScalar restitutionCurveInternal(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold);
static btSolverConstraint& addTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity = 0, btScalar cfmSlip = 0.);
static void setupTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity, btScalar cfmSlip);
static void setupFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip);
static btSolverConstraint& addFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
static void setFrictionConstraintImpulseInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool,
btSolverConstraint& solverConstraint,
int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
static void convertJointInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool,
int& maxOverrideNumSolverIterations,
btSolverConstraint* currentConstraintRow,
btTypedConstraint* constraint,
const btTypedConstraint::btConstraintInfo1& info1,
int solverBodyIdA,
int solverBodyIdB,
const btContactSolverInfo& infoGlobal);
static btScalar solveGroupCacheFriendlyFinishInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
static void writeBackContactsInternal(btConstraintArray& tmpSolverContactConstraintPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
static void writeBackJointsInternal(btConstraintArray& tmpSolverNonContactConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
static void writeBackBodiesInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
static void solveGroupCacheFriendlySplitImpulseIterationsInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
///clear internal cached data and reset random seed
virtual void reset();
unsigned long btRand2();
int btRandInt2(int n);
static unsigned long btRand2a(unsigned long& seed);
static int btRandInt2a(int n, unsigned long& seed);
int btRandInt2(int n);
void setRandSeed(unsigned long seed)
{
@@ -305,18 +201,14 @@ public:
///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
static btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
static btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4
static btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
static btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric();
static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric();
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
btSolverAnalyticsData m_analyticsData;
};

21
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h
View File

@@ -46,31 +46,14 @@ protected:
void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
{
btSISolverSingleIterationData siData(m_tmpSolverBodyPool,
m_tmpSolverContactConstraintPool,
m_tmpSolverNonContactConstraintPool,
m_tmpSolverContactFrictionConstraintPool,
m_tmpSolverContactRollingFrictionConstraintPool,
m_orderTmpConstraintPool,
m_orderNonContactConstraintPool,
m_orderFrictionConstraintPool,
m_tmpConstraintSizesPool,
m_resolveSingleConstraintRowGeneric,
m_resolveSingleConstraintRowLowerLimit,
m_resolveSplitPenetrationImpulse,
m_kinematicBodyUniqueIdToSolverBodyTable,
m_btSeed2,
m_fixedBodyId,
m_maxOverrideNumSolverIterations);
for (int i = 0; i < numBodies; i++)
{
int bodyId = siData.getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
btRigidBody* body = btRigidBody::upcast(bodies[i]);
if (body && body->getInvMass())
{
btSolverBody& solverBody = siData.m_tmpSolverBodyPool[bodyId];
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
}