From a1c962192c37eb5ca5ce46d34d1c1e7318605a8d Mon Sep 17 00:00:00 2001 From: "erwin.coumans@gmail.com" Date: Wed, 23 Oct 2013 23:35:13 +0000 Subject: [PATCH] Allow the ForkLiftDemo to toggle between MLCP and SI solver, using F6 key. Apply patch for CMake config, see Issue 754 (Issue 753) Fix a few issue with the MLCP solver: allow split impulse, and fix offset in friction dependencies --- BulletConfig.cmake.in | 25 +++++ CMakeLists.txt | 15 +++ ChangeLog | 4 + Demos/ForkLiftDemo/ForkLiftDemo.cpp | 104 ++++++++++++++++-- Demos/ForkLiftDemo/ForkLiftDemo.h | 6 +- Demos/OpenGL/DemoApplication.cpp | 2 +- UseBullet.cmake | 10 ++ src/BulletDynamics/CMakeLists.txt | 14 +++ .../ConstraintSolver/btConstraintSolver.h | 12 ++ .../btSequentialImpulseConstraintSolver.h | 5 + .../MLCPSolvers/btDantzigLCP.cpp | 4 +- .../MLCPSolvers/btDantzigSolver.h | 57 ++++++---- .../MLCPSolvers/btMLCPSolver.cpp | 63 +++++++++-- src/BulletDynamics/MLCPSolvers/btMLCPSolver.h | 25 ++++- .../MLCPSolvers/btSolveProjectedGaussSeidel.h | 1 + 15 files changed, 298 insertions(+), 49 deletions(-) create mode 100644 BulletConfig.cmake.in create mode 100644 UseBullet.cmake diff --git a/BulletConfig.cmake.in b/BulletConfig.cmake.in new file mode 100644 index 000000000..da2dfa5dc --- /dev/null +++ b/BulletConfig.cmake.in @@ -0,0 +1,25 @@ +# -*- cmake -*- +# +# BulletConfig.cmake(.in) +# + +# Use the following variables to compile and link against Bullet: +# BULLET_FOUND - True if Bullet was found on your system +# BULLET_USE_FILE - The file making Bullet usable +# BULLET_DEFINITIONS - Definitions needed to build with Bullet +# BULLET_INCLUDE_DIR - Directory where Bullet-C-Api.h can be found +# BULLET_INCLUDE_DIRS - List of directories of Bullet and it's dependencies +# BULLET_LIBRARIES - List of libraries to link against Bullet library +# BULLET_LIBRARY_DIRS - List of directories containing Bullet' libraries +# BULLET_ROOT_DIR - The base directory of Bullet +# BULLET_VERSION_STRING - A human-readable string containing the version + +set ( BULLET_FOUND 1 ) +set ( BULLET_USE_FILE "@BULLET_USE_FILE@" ) +set ( BULLET_DEFINITIONS "@BULLET_DEFINITIONS@" ) +set ( BULLET_INCLUDE_DIR "@INCLUDE_INSTALL_DIR@" ) +set ( BULLET_INCLUDE_DIRS "@INCLUDE_INSTALL_DIR@" ) +set ( BULLET_LIBRARIES "@BULLET_LIBRARIES@" ) +set ( BULLET_LIBRARY_DIRS "@LIB_DESTINATION@" ) +set ( BULLET_ROOT_DIR "@CMAKE_INSTALL_PREFIX@" ) +set ( BULLET_VERSION_STRING "@BULLET_VERSION@" ) \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 4dab1110d..18a089a9e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -422,3 +422,18 @@ OPTION(BUILD_UNIT_TESTS "Build Unit Tests" OFF) IF (BUILD_UNIT_TESTS) SUBDIRS(UnitTests) ENDIF() + +set (BULLET_CONFIG_CMAKE_PATH lib${LIB_SUFFIX}/cmake/bullet ) +list (APPEND BULLET_LIBRARIES LinearMath) +list (APPEND BULLET_LIBRARIES BulletCollisions) +list (APPEND BULLET_LIBRARIES BulletDynamics) +list (APPEND BULLET_LIBRARIES BulletSoftBody) +set (BULLET_USE_FILE ${CMAKE_INSTALL_PREFIX}/${BULLET_CONFIG_CMAKE_PATH}/UseBullet.cmake) +configure_file ( ${CMAKE_SOURCE_DIR}/BulletConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/BulletConfig.cmake + @ONLY ESCAPE_QUOTES + ) +install ( FILES ${CMAKE_SOURCE_DIR}/UseBullet.cmake + ${CMAKE_CURRENT_BINARY_DIR}/BulletConfig.cmake + DESTINATION ${BULLET_CONFIG_CMAKE_PATH} + ) diff --git a/ChangeLog b/ChangeLog index 4a0ccf492..f5c85c801 100644 --- a/ChangeLog +++ b/ChangeLog @@ -4,6 +4,10 @@ Primary author and maintainer: Erwin Coumans This ChangeLog is incomplete, for an up-to-date list of all fixed issues see http://bullet.googlecode.com using http://tinyurl.com/yabmjjj +2013 October 23 + - Bullet 2.82 release + - See docs/BulletQuickstart.pdf or issue tracked for details. + 2012 September 10 - Bullet 2.81 release preparation diff --git a/Demos/ForkLiftDemo/ForkLiftDemo.cpp b/Demos/ForkLiftDemo/ForkLiftDemo.cpp index 30bb7f5e9..093e0e127 100644 --- a/Demos/ForkLiftDemo/ForkLiftDemo.cpp +++ b/Demos/ForkLiftDemo/ForkLiftDemo.cpp @@ -22,6 +22,11 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" #include "GLDebugFont.h" +#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" +#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" +#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h" + + #ifndef M_PI #define M_PI 3.14159265358979323846 #endif @@ -55,6 +60,8 @@ subject to the following restrictions: btVector3 wheelAxleCS(-1,0,0); #endif +bool useMCLPSolver = true; + #include "GLDebugDrawer.h" #include //printf debugging @@ -123,7 +130,7 @@ m_maxCameraDistance(10.f) } -void ForkLiftDemo::termPhysics() +void ForkLiftDemo::exitPhysics() { //cleanup in the reverse order of creation/initialization @@ -157,6 +164,7 @@ void ForkLiftDemo::termPhysics() btCollisionShape* shape = m_collisionShapes[j]; delete shape; } + m_collisionShapes.clear(); delete m_indexVertexArrays; delete m_vertices; @@ -185,7 +193,7 @@ void ForkLiftDemo::termPhysics() ForkLiftDemo::~ForkLiftDemo() { - termPhysics(); + exitPhysics(); } void ForkLiftDemo::initPhysics() @@ -203,8 +211,24 @@ void ForkLiftDemo::initPhysics() btVector3 worldMin(-1000,-1000,-1000); btVector3 worldMax(1000,1000,1000); m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax); - m_constraintSolver = new btSequentialImpulseConstraintSolver(); + if (useMCLPSolver) + { + btDantzigSolver* mlcp = new btDantzigSolver(); + //btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel; + btMLCPSolver* sol = new btMLCPSolver(mlcp); + m_constraintSolver = sol; + } else + { + m_constraintSolver = new btSequentialImpulseConstraintSolver(); + } m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration); + if (useMCLPSolver) + { + m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 1;//for direct solver it is better to have a small A matrix + } else + { + m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 128;//for direct solver, it is better to solve multiple objects together, small batches have high overhead + } #ifdef FORCE_ZAXIS_UP m_dynamicsWorld->setGravity(btVector3(0,0,-10)); #endif @@ -212,7 +236,7 @@ void ForkLiftDemo::initPhysics() //m_dynamicsWorld->setGravity(btVector3(0,0,0)); btTransform tr; tr.setIdentity(); -tr.setOrigin(btVector3(0,-10,0)); +tr.setOrigin(btVector3(0,-3,0)); //either use heightfield or triangle mesh @@ -341,15 +365,15 @@ tr.setOrigin(btVector3(0,-10,0)); loadTrans.setOrigin(btVector3(-2.1f, 0.0f, 0.0f)); loadCompound->addChildShape(loadTrans, loadShapeC); loadTrans.setIdentity(); - m_loadStartPos = btVector3(0.0f, -3.5f, 7.0f); + m_loadStartPos = btVector3(0.0f, 3.5f, 7.0f); loadTrans.setOrigin(m_loadStartPos); m_loadBody = localCreateRigidBody(4, loadTrans, loadCompound); } - clientResetScene(); - + + /// create vehicle { @@ -408,6 +432,7 @@ tr.setOrigin(btVector3(0,-10,0)); } } + resetForklift(); setCameraDistance(26.f); @@ -459,6 +484,25 @@ void ForkLiftDemo::renderme() sprintf(buf,"SHIFT+Cursor UP/Down - move fork up/down"); yStart+=20; GLDebugDrawString(xStart,yStart,buf); + + yStart+=20; + glRasterPos3f(xStart, yStart, 0); + sprintf(buf,"F6 - toggle solver"); + GLDebugDrawString(xStart,yStart,buf); + + yStart+=20; + glRasterPos3f(xStart, yStart, 0); + if (m_dynamicsWorld->getConstraintSolver()->getSolverType()==BT_MLCP_SOLVER) + { + sprintf(buf,"Using direct MLCP solver"); + } else + { + sprintf(buf,"Using sequential impulse solver"); + } + GLDebugDrawString(xStart,yStart,buf); + + + glRasterPos3f(xStart, yStart, 0); sprintf(buf,"F5 - toggle camera mode"); yStart+=20; @@ -510,6 +554,19 @@ void ForkLiftDemo::clientMoveAndDisplay() int numSimSteps; numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps); + if (m_dynamicsWorld->getConstraintSolver()->getSolverType()==BT_MLCP_SOLVER) + { + btMLCPSolver* sol = (btMLCPSolver*) m_dynamicsWorld->getConstraintSolver(); + int numFallbacks = sol->getNumFallbacks(); + if (numFallbacks) + { + static int totalFailures = 0; + totalFailures+=numFallbacks; + printf("MLCP solver failed %d times, falling back to btSequentialImpulseSolver (SI)\n",totalFailures); + } + sol->setNumFallbacks(0); + } + //#define VERBOSE_FEEDBACK #ifdef VERBOSE_FEEDBACK @@ -574,8 +631,13 @@ void ForkLiftDemo::displayCallback(void) } - void ForkLiftDemo::clientResetScene() +{ + exitPhysics(); + initPhysics(); +} + +void ForkLiftDemo::resetForklift() { gVehicleSteering = 0.f; gBreakingForce = defaultBreakingForce; @@ -747,6 +809,32 @@ void ForkLiftDemo::specialKeyboard(int key, int x, int y) break; } + case GLUT_KEY_F6: + { + //switch solver (needs demo restart) + useMCLPSolver = !useMCLPSolver; + printf("switching to useMLCPSolver = %d\n", useMCLPSolver); + + delete m_constraintSolver; + if (useMCLPSolver) + { + btDantzigSolver* mlcp = new btDantzigSolver(); + //btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel; + btMLCPSolver* sol = new btMLCPSolver(mlcp); + m_constraintSolver = sol; + } else + { + m_constraintSolver = new btSequentialImpulseConstraintSolver(); + } + + m_dynamicsWorld->setConstraintSolver(m_constraintSolver); + + + //exitPhysics(); + //initPhysics(); + break; + } + case GLUT_KEY_F5: m_useDefaultCamera = !m_useDefaultCamera; break; diff --git a/Demos/ForkLiftDemo/ForkLiftDemo.h b/Demos/ForkLiftDemo/ForkLiftDemo.h index 6d0c8255a..3489f3732 100644 --- a/Demos/ForkLiftDemo/ForkLiftDemo.h +++ b/Demos/ForkLiftDemo/ForkLiftDemo.h @@ -83,7 +83,9 @@ class ForkLiftDemo : public GlutDemoApplication virtual void clientMoveAndDisplay(); - virtual void clientResetScene(); + virtual void resetForklift(); + + virtual void clientResetScene(); virtual void displayCallback(); @@ -97,7 +99,7 @@ class ForkLiftDemo : public GlutDemoApplication void renderme(); void initPhysics(); - void termPhysics(); + void exitPhysics(); static DemoApplication* Create() { diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp index 18f2dc2fc..3baaca44c 100644 --- a/Demos/OpenGL/DemoApplication.cpp +++ b/Demos/OpenGL/DemoApplication.cpp @@ -817,7 +817,7 @@ void DemoApplication::pickObject(const btVector3& pickPos, const btCollisionObje btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; - if ((m_modifierKeys& BT_ACTIVE_SHIFT)==0) + if ((m_modifierKeys& BT_ACTIVE_SHIFT)!=0) { btTransform tr; tr.setIdentity(); diff --git a/UseBullet.cmake b/UseBullet.cmake new file mode 100644 index 000000000..5ed94874a --- /dev/null +++ b/UseBullet.cmake @@ -0,0 +1,10 @@ +# -*- cmake -*- +# +# UseBullet.cmake +# + + +add_definitions ( ${BULLET_DEFINITIONS} ) +include_directories ( ${BULLET_INCLUDE_DIRS} ) +link_directories ( ${BULLET_LIBRARY_DIRS} ) + diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index f09ce3f84..cc4727639 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -31,6 +31,8 @@ SET(BulletDynamics_SRCS Featherstone/btMultiBodyConstraint.cpp Featherstone/btMultiBodyPoint2Point.cpp Featherstone/btMultiBodyJointMotor.cpp + MLCPSolvers/btDantzigLCP.cpp + MLCPSolvers/btMLCPSolver.cpp ) SET(Root_HDRS @@ -84,6 +86,16 @@ SET(Featherstone_HDRS Featherstone/btMultiBodyPoint2Point.h Featherstone/btMultiBodyJointMotor.h ) + +SET(MLCPSolvers_HDRS + MLCPSolvers/btDantzigLCP.h + MLCPSolvers/btDantzigSolver.h + MLCPSolvers/btMLCPSolver.h + MLCPSolvers/btMLCPSolverInterface.h + MLCPSolvers/btPATHSolver.h + MLCPSolvers/btSolveProjectedGaussSeidel.h +) + SET(Character_HDRS Character/btCharacterControllerInterface.h Character/btKinematicCharacterController.h @@ -98,6 +110,7 @@ SET(BulletDynamics_HDRS ${Vehicle_HDRS} ${Character_HDRS} ${Featherstone_HDRS} + ${MLCPSolvers_HDRS} ) @@ -134,6 +147,7 @@ DESTINATION ${INCLUDE_INSTALL_DIR}/BulletDynamics) SET_PROPERTY(SOURCE ${Vehicle_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Vehicle) SET_PROPERTY(SOURCE ${Character_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Character) SET_PROPERTY(SOURCE ${Featherstone_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Featherstone) + SET_PROPERTY(SOURCE ${MLCPSolvers_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/MLCPSolvers) ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) ENDIF (INSTALL_LIBS) diff --git a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index ddb512679..1ba1cd1e8 100644 --- a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -28,6 +28,14 @@ class btIDebugDraw; class btStackAlloc; class btDispatcher; /// btConstraintSolver provides solver interface + + +enum btConstraintSolverType +{ + BT_SEQUENTIAL_IMPULSE_SOLVER=1, + BT_MLCP_SOLVER=2 +}; + class btConstraintSolver { @@ -44,6 +52,10 @@ public: ///clear internal cached data and reset random seed virtual void reset() = 0; + + virtual btConstraintSolverType getSolverType() const=0; + + }; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index f2f3b3bd7..180d2a385 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -134,6 +134,11 @@ public: return m_btSeed2; } + + virtual btConstraintSolverType getSolverType() const + { + return BT_SEQUENTIAL_IMPULSE_SOLVER; + } }; diff --git a/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp b/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp index 1794b18e3..ad604cdf1 100644 --- a/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp +++ b/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp @@ -1782,7 +1782,7 @@ bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, { s_error = false; - printf("btSolveDantzigLCP n=%d\n",n); +// printf("btSolveDantzigLCP n=%d\n",n); btAssert (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n); #ifdef BT_DEBUG { @@ -2011,7 +2011,7 @@ bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, // our fingers and exit with the current solution. if (s <= btScalar(0.0)) { - printf("LCP internal error, s <= 0 (s=%.4e)",(double)s); +// printf("LCP internal error, s <= 0 (s=%.4e)",(double)s); if (i < n) { btSetZero (x+i,n-i); btSetZero (w+i,n-i); diff --git a/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h b/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h index bcbc629c4..8ad36967b 100644 --- a/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h +++ b/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h @@ -28,6 +28,14 @@ protected: btScalar m_acceptableUpperLimitSolution; btAlignedObjectArray m_tempBuffer; + + btAlignedObjectArray m_A; + btAlignedObjectArray m_b; + btAlignedObjectArray m_x; + btAlignedObjectArray m_lo; + btAlignedObjectArray m_hi; + btAlignedObjectArray m_dependencies; + public: btDantzigSolver() @@ -41,47 +49,46 @@ public: int n = b.rows(); if (n) { - btScalar* AA = (btScalar*) A.getBufferPointer(); - btScalar* bb = (btScalar* ) b.getBufferPointer(); - btScalar* xx = (btScalar*) x.getBufferPointer(); - btScalar* llo = (btScalar*) lo.getBufferPointer(); - btScalar* hhi = (btScalar*) hi.getBufferPointer(); - int* findex = (int*) &limitDependency[0]; int nub = 0; btAlignedObjectArray ww; ww.resize(n); - - + const btScalar* Aptr = A.getBufferPointer(); - + m_A.resize(n*n); for (int i=0;i= m_acceptableUpperLimitSolution) { return false; @@ -91,7 +98,11 @@ public: { return false; } - + } + + for (int i=0;isolveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + bool result = true; + + if (m_A.rows()==0) + return true; + + //if using split impulse, we solve 2 separate (M)LCPs + if (infoGlobal.m_splitImpulse) + { + btMatrixXu Acopy = m_A; + btAlignedObjectArray limitDependenciesCopy = m_limitDependencies; +// printf("solve first LCP\n"); + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + if (result) + result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations ); + + } else + { + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + } + return result; } struct btJointNode @@ -139,11 +159,16 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { BT_PROFILE("init b (rhs)"); m_b.resize(numConstraintRows); + m_bSplit.resize(numConstraintRows); //m_b.setZero(); for (int i=0;i m_limitDependencies; btConstraintArray m_allConstraintArray; - btMLCPSolverInterface* m_solver; + int m_fallback; 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); @@ -56,6 +61,20 @@ public: m_solver = solver; } + int getNumFallbacks() const + { + return m_fallback; + } + void setNumFallbacks(int num) + { + m_fallback = num; + } + + virtual btConstraintSolverType getSolverType() const + { + return BT_MLCP_SOLVER; + } + }; diff --git a/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h index 85166e391..ce8a07e64 100644 --- a/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h +++ b/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h @@ -72,6 +72,7 @@ public: x[i]=hi[i]*s; } } + return true; } };