From 25c61a40b515f6a3d68e3e68c970d30bf408509a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 11 Sep 2019 10:19:43 -0700 Subject: [PATCH] remove BlockSolver/* --- examples/BlockSolver/BlockSolverExample.cpp | 209 ----------- examples/BlockSolver/BlockSolverExample.h | 7 - examples/BlockSolver/RigidBodyBoxes.cpp | 151 -------- examples/BlockSolver/RigidBodyBoxes.h | 6 - examples/BlockSolver/btBlockSolver.cpp | 374 -------------------- examples/BlockSolver/btBlockSolver.h | 77 ---- examples/ExampleBrowser/ExampleEntries.cpp | 10 - 7 files changed, 834 deletions(-) delete mode 100644 examples/BlockSolver/BlockSolverExample.cpp delete mode 100644 examples/BlockSolver/BlockSolverExample.h delete mode 100644 examples/BlockSolver/RigidBodyBoxes.cpp delete mode 100644 examples/BlockSolver/RigidBodyBoxes.h delete mode 100644 examples/BlockSolver/btBlockSolver.cpp delete mode 100644 examples/BlockSolver/btBlockSolver.h diff --git a/examples/BlockSolver/BlockSolverExample.cpp b/examples/BlockSolver/BlockSolverExample.cpp deleted file mode 100644 index 61685079a..000000000 --- a/examples/BlockSolver/BlockSolverExample.cpp +++ /dev/null @@ -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); -} diff --git a/examples/BlockSolver/BlockSolverExample.h b/examples/BlockSolver/BlockSolverExample.h deleted file mode 100644 index faa11d885..000000000 --- a/examples/BlockSolver/BlockSolverExample.h +++ /dev/null @@ -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 diff --git a/examples/BlockSolver/RigidBodyBoxes.cpp b/examples/BlockSolver/RigidBodyBoxes.cpp deleted file mode 100644 index e475413be..000000000 --- a/examples/BlockSolver/RigidBodyBoxes.cpp +++ /dev/null @@ -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 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); -} diff --git a/examples/BlockSolver/RigidBodyBoxes.h b/examples/BlockSolver/RigidBodyBoxes.h deleted file mode 100644 index 0b6e651d6..000000000 --- a/examples/BlockSolver/RigidBodyBoxes.h +++ /dev/null @@ -1,6 +0,0 @@ -#ifndef BLOCKSOLVER_RIGIDBODYBOXES_H_ -#define BLOCKSOLVER_RIGIDBODYBOXES_H_ - -class CommonExampleInterface* RigidBodyBoxesCreateFunc(struct CommonExampleOptions& options); - -#endif //BLOCKSOLVER_RIGIDBODYBOXES_H_ diff --git a/examples/BlockSolver/btBlockSolver.cpp b/examples/BlockSolver/btBlockSolver.cpp deleted file mode 100644 index 8cf144c14..000000000 --- a/examples/BlockSolver/btBlockSolver.cpp +++ /dev/null @@ -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 m_tmpSolverBodyPool; - btConstraintArray m_tmpSolverContactConstraintPool; - btConstraintArray m_tmpSolverNonContactConstraintPool; - btConstraintArray m_tmpSolverContactFrictionConstraintPool; - btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool; - - btAlignedObjectArray m_orderTmpConstraintPool; - btAlignedObjectArray m_orderNonContactConstraintPool; - btAlignedObjectArray m_orderFrictionConstraintPool; - btAlignedObjectArray - m_tmpConstraintSizesPool; - - unsigned long m_btSeed2; - int m_fixedBodyId; - int m_maxOverrideNumSolverIterations; - btAlignedObjectArray - 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; -} diff --git a/examples/BlockSolver/btBlockSolver.h b/examples/BlockSolver/btBlockSolver.h deleted file mode 100644 index 271ad399d..000000000 --- a/examples/BlockSolver/btBlockSolver.h +++ /dev/null @@ -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 diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 2beef333c..1c8fa7921 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -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),