remove BlockSolver/*
This commit is contained in:
Erwin Coumans
@@ -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);
|
||||
}
|
||||
@@ -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
|
||||
@@ -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);
|
||||
}
|
||||
@@ -1,6 +0,0 @@
|
||||
#ifndef BLOCKSOLVER_RIGIDBODYBOXES_H_
|
||||
#define BLOCKSOLVER_RIGIDBODYBOXES_H_
|
||||
|
||||
class CommonExampleInterface* RigidBodyBoxesCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //BLOCKSOLVER_RIGIDBODYBOXES_H_
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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
|
||||
@@ -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),
|
||||
|
||||
Reference in New Issue
Block a user