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

preparation for block solver btRigidBody.

Browse Source
This commit is contained in:
erwincoumans
2019-02-26 20:24:15 -08:00
parent 09dbb8ba1b
commit c44471c38c
13 changed files with 1377 additions and 188 deletions
Download Patch File Download Diff File Expand all files Collapse all files

380
examples/BlockSolver/BlockSolverExample.cpp Normal file
View File

@@ -0,0 +1,380 @@
#include "BlockSolverExample.h"
#include "../OpenGLWindow/SimpleOpenGL3App.h"
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
#include "btBlockSolver.h"
#include "../OpenGLWindow/GLInstancingRenderer.h"
#include "BulletCollision/CollisionShapes/btShapeHull.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 = 1;
float pitch = -35;
float yaw = 50;
float targetPos[3] = {-3, 2.8, -2.5};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool fixedBase = false);
void createGround(const btVector3& halfExtents = btVector3(50, 50, 50), btScalar zOffSet = btScalar(-1.55));
void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
};
static bool g_fixedBase = true;
static bool g_firstInit = true;
static float scaling = 0.4f;
static float friction = 1.;
BlockSolverExample::BlockSolverExample(GUIHelperInterface* helper, int option)
: CommonMultiBodyBase(helper),
m_option(option)
{
m_guiHelper->setUpAxis(1);
}
BlockSolverExample::~BlockSolverExample()
{
// Do nothing
}
void BlockSolverExample::stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
}
void BlockSolverExample::initPhysics()
{
m_guiHelper->setUpAxis(1);
if (g_firstInit)
{
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10. * scaling));
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50);
g_firstInit = false;
}
///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, -10, 0));
m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good?
/////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////
bool damping = true;
bool gyro = true;
int numLinks = 5;
bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool multibodyOnly = true; //false
bool canSleep = true;
bool selfCollide = true;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
btMultiBody* mbC1 = createFeatherstoneMultiBody(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase);
btMultiBody* mbC2 = createFeatherstoneMultiBody(world, numLinks, btVector3(-0.4f, 3.0f, 0.5f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase);
mbC1->setCanSleep(canSleep);
mbC1->setHasSelfCollision(selfCollide);
mbC1->setUseGyroTerm(gyro);
if (!damping)
{
mbC1->setLinearDamping(0.f);
mbC1->setAngularDamping(0.f);
}
else
{
mbC1->setLinearDamping(0.1f);
mbC1->setAngularDamping(0.9f);
}
//
m_dynamicsWorld->setGravity(btVector3(0, -9.81, 0));
//////////////////////////////////////////////
if (numLinks > 0)
{
btScalar q0 = 45.f * SIMD_PI / 180.f;
if (!spherical)
{
mbC1->setJointPosMultiDof(0, &q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC1->setJointPosMultiDof(0, quat0);
}
}
///
addColliders(mbC1, world, baseHalfExtents, linkHalfExtents);
mbC2->setCanSleep(canSleep);
mbC2->setHasSelfCollision(selfCollide);
mbC2->setUseGyroTerm(gyro);
//
if (!damping)
{
mbC2->setLinearDamping(0.f);
mbC2->setAngularDamping(0.f);
}
else
{
mbC2->setLinearDamping(0.1f);
mbC2->setAngularDamping(0.9f);
}
//
m_dynamicsWorld->setGravity(btVector3(0, -9.81, 0));
//////////////////////////////////////////////
if (numLinks > 0)
{
btScalar q0 = -45.f * SIMD_PI / 180.f;
if (!spherical)
{
mbC2->setJointPosMultiDof(0, &q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC2->setJointPosMultiDof(0, quat0);
}
}
///
addColliders(mbC2, world, baseHalfExtents, linkHalfExtents);
/////////////////////////////////////////////////////////////////
btScalar groundHeight = -51.55;
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
createGround();
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
/////////////////////////////////////////////////////////////////
}
btMultiBody* BlockSolverExample::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool fixedBase)
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
if (baseMass)
{
btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
bool canSleep = false;
btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, fixedBase, canSleep);
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
float linkMass = 1.f;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI / 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
if (!spherical)
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
else
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
}
pMultiBody->finalizeMultiDof();
///
pWorld->addMultiBody(pMultiBody);
///
return pMultiBody;
}
void BlockSolverExample::createGround(const btVector3& halfExtents, btScalar zOffSet)
{
btCollisionShape* groundShape = new btBoxShape(halfExtents);
m_collisionShapes.push_back(groundShape);
// rigidbody is dynamic if and only if mass is non zero, otherwise static
btScalar mass(0.);
const bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
groundShape->calculateLocalInertia(mass, localInertia);
// using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -halfExtents.z() + zOffSet, 0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
// add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body, 1, 1 + 2);
}
void BlockSolverExample::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
{
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
{
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
if (1)
{
btCollisionShape* box = new btBoxShape(baseHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
pWorld->addCollisionObject(col, 2, 1 + 2);
col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i + 1];
btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
col->setFriction(friction);
pWorld->addCollisionObject(col, 2, 1 + 2);
pMultiBody->getLink(i).m_collider = col;
}
}
CommonExampleInterface* BlockSolverExampleCreateFunc(CommonExampleOptions& options)
{
return new BlockSolverExample(options.m_guiHelper, options.m_option);
}

19
examples/BlockSolver/BlockSolverExample.h Normal file
View File

@@ -0,0 +1,19 @@
#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_STACK= 1 << 5,
BLOCK_SOLVER_SCENE_CHAIN = 1<< 6,
};
class CommonExampleInterface* BlockSolverExampleCreateFunc(struct CommonExampleOptions& options);
#endif //BLOCK_SOLVER_EXAMPLE_H

161
examples/BlockSolver/btBlockSolver.cpp Normal file
View File

@@ -0,0 +1,161 @@
#include "btBlockSolver.h"
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "LinearMath/btQuickprof.h"
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_data = new btBlockSolverInternalData;
}
btBlockSolver::~btBlockSolver()
{
delete m_data;
}
btScalar btBlockSolver::solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
{
btSISolverSingleIterationData siData(m_data->m_tmpSolverBodyPool,
m_data->m_tmpSolverContactConstraintPool,
m_data->m_tmpSolverNonContactConstraintPool,
m_data->m_tmpSolverContactFrictionConstraintPool,
m_data->m_tmpSolverContactRollingFrictionConstraintPool,
m_data->m_orderTmpConstraintPool,
m_data->m_orderNonContactConstraintPool,
m_data->m_orderFrictionConstraintPool,
m_data->m_tmpConstraintSizesPool,
m_data->m_resolveSingleConstraintRowGeneric,
m_data->m_resolveSingleConstraintRowLowerLimit,
m_data->m_resolveSplitPenetrationImpulse,
m_data->m_kinematicBodyUniqueIdToSolverBodyTable,
m_data->m_btSeed2,
m_data->m_fixedBodyId,
m_data->m_maxOverrideNumSolverIterations);
m_data->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++)
//for ( int iteration = maxIterations-1 ; iteration >= 0;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_data->m_btSeed2=0?
delete m_data;
m_data = new btBlockSolverInternalData;
}

29
examples/BlockSolver/btBlockSolver.h Normal file
View File

@@ -0,0 +1,29 @@
#ifndef BT_BLOCK_SOLVER_H
#define BT_BLOCK_SOLVER_H
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
class btBlockSolver : public btConstraintSolver
{
struct btBlockSolverInternalData* m_data;
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);
//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

15
examples/Evolution/NN3DWalkersTimeWarpBase.h
View File

@@ -91,7 +91,7 @@ enum SolverEnumType
NNCGSOLVER = 2,
DANZIGSOLVER = 3,
LEMKESOLVER = 4,
FSSOLVER = 5,
NUM_SOLVERS = 6
};
@@ -103,7 +103,7 @@ static char GAUSSSEIDELSOLVER[] = "Gauss-Seidel Solver";
static char NNCGSOLVER[] = "NNCG Solver";
static char DANZIGSOLVER[] = "Danzig Solver";
static char LEMKESOLVER[] = "Lemke Solver";
static char FSSOLVER[] = "FeatherStone Solver";
}; // namespace SolverType
static const char* solverTypes[NUM_SOLVERS];
@@ -324,7 +324,7 @@ struct NN3DWalkersTimeWarpBase : public CommonRigidBodyBase
solverTypes[2] = SolverType::NNCGSOLVER;
solverTypes[3] = SolverType::DANZIGSOLVER;
solverTypes[4] = SolverType::LEMKESOLVER;
solverTypes[5] = SolverType::FSSOLVER;
{
ComboBoxParams comboParams;
@@ -499,19 +499,12 @@ struct NN3DWalkersTimeWarpBase : public CommonRigidBodyBase
m_solver = new btMLCPSolver(mlcp);
break;
}
case FSSOLVER:
{
// b3Printf("=%s=",SolverType::FSSOLVER);
//Use the btMultiBodyConstraintSolver for Featherstone btMultiBody support
m_solver = new btMultiBodyConstraintSolver;
break;
}
default:
break;
}
if (SOLVER_TYPE != FSSOLVER)
if (1)
{
//TODO: Set parameters for other solvers

4
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -202,6 +202,10 @@ SET(BulletExampleBrowser_SRCS
../MultiThreadedDemo/MultiThreadedDemo.h
../MultiThreadedDemo/CommonRigidBodyMTBase.cpp
../MultiThreadedDemo/CommonRigidBodyMTBase.h
../BlockSolver/btBlockSolver.cpp
../BlockSolver/btBlockSolver.h
../BlockSolver/BlockSolverExample.cpp
../BlockSolver/BlockSolverExample.h
../Tutorial/Tutorial.cpp
../Tutorial/Tutorial.h
../Tutorial/Dof6ConstraintTutorial.cpp

8
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -1,5 +1,6 @@
#include "ExampleEntries.h"
#include "../BlockSolver/BlockSolverExample.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "EmptyExample.h"
#include "../RenderingExamples/RenderInstancingDemo.h"
@@ -150,6 +151,13 @@ 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_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_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_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_STACK + 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),
ExampleEntry(1, "Inverse Dynamics Prog", "Create a btMultiBody programatically. 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_PROGRAMMATICALLY),

1
examples/ExampleBrowser/premake4.lua
View File

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

1
src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
View File

@@ -35,6 +35,7 @@ enum btConstraintSolverType
BT_MLCP_SOLVER = 2,
BT_NNCG_SOLVER = 4,
BT_MULTIBODY_SOLVER = 8,
BT_BLOCK_SOLVER = 16,
};
class btConstraintSolver

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

File diff suppressed because it is too large Load Diff

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

@@ -29,6 +29,68 @@ 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_resolveSingleConstraintRowGeneric(resolveSingleConstraintRowGeneric),
m_resolveSingleConstraintRowLowerLimit(resolveSingleConstraintRowLowerLimit),
m_resolveSplitPenetrationImpulse(resolveSplitPenetrationImpulse),
m_kinematicBodyUniqueIdToSolverBodyTable(kinematicBodyUniqueIdToSolverBodyTable),
m_seed(seed),
m_fixedBodyId(fixedBodyId),
m_maxOverrideNumSolverIterations(maxOverrideNumSolverIterations)
{
}
};
///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
ATTRIBUTE_ALIGNED16(class)
btSequentialImpulseConstraintSolver : public btConstraintSolver
@@ -97,6 +159,7 @@ 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)
@@ -123,6 +186,7 @@ protected:
}
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);
@@ -130,6 +194,7 @@ protected:
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);
@@ -141,13 +206,52 @@ 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);
void setRandSeed(unsigned long seed)
{
m_btSeed2 = seed;
@@ -180,14 +284,18 @@ public:
}
///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
static btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
static btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
static btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
static btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric();
static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric();
};
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H

4
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -362,7 +362,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
};
btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
: btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
: btDiscreteDynamicsWorld(dispatcher, pairCache, 0, collisionConfiguration),
m_multiBodyConstraintSolver(constraintSolver)
{
//split impulse is not yet supported for Featherstone hierarchies
@@ -380,7 +380,7 @@ void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstrain
{
m_multiBodyConstraintSolver = solver;
m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver);
btDiscreteDynamicsWorld::setConstraintSolver(solver);
//btDiscreteDynamicsWorld::setConstraintSolver(solver);
}
void btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)

2
src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h
View File

@@ -156,7 +156,7 @@ protected:
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& infoGlobal,
btIDebugDraw* debugDrawer) BT_OVERRIDE;
btIDebugDraw* debugDrawer) ;
public:
BT_DECLARE_ALIGNED_ALLOCATOR()