From 046a86cd845bc6a1ce6cabb2db5ede33275ec4ce Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 8 Jun 2018 19:14:03 -0700 Subject: [PATCH 1/2] add premake4.lua build for BulletRobotics target, C++ API, similar to PyBullet. Use BulletRobotics target for App_HelloBulletRobotics (without GUI) and App_RobotSimulator --- Extras/BulletRobotics/premake4.lua | 179 +++++++++++++++++++++++++++ Extras/premake4.lua | 1 + examples/RobotSimulator/premake4.lua | 153 ++++++++++------------- 3 files changed, 248 insertions(+), 85 deletions(-) create mode 100644 Extras/BulletRobotics/premake4.lua diff --git a/Extras/BulletRobotics/premake4.lua b/Extras/BulletRobotics/premake4.lua new file mode 100644 index 000000000..b1e942ce5 --- /dev/null +++ b/Extras/BulletRobotics/premake4.lua @@ -0,0 +1,179 @@ + + +project ("BulletRobotics") + language "C++" + kind "StaticLib" + + includedirs {"../../src", "../../examples", + "../../examples/ThirdPartyLibs"} + defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"} + hasCL = findOpenCL("clew") + + links{"BulletExampleBrowserLib","gwen", "BulletFileLoader","BulletWorldImporter","OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK", "Bullet3Common"} + initOpenGL() + initGlew() + + includedirs { + "../../src", + "../../examples", + "../../examples/SharedMemory", + "../ThirdPartyLibs", + "../ThirdPartyLibs/enet/include", + "../ThirdPartyLibs/clsocket/src", + } + + if os.is("MacOSX") then +-- targetextension {"so"} + links{"Cocoa.framework","Python"} + end + + +if not _OPTIONS["no-enet"] then + + includedirs {"../../examples/ThirdPartyLibs/enet/include"} + + if os.is("Windows") then +-- targetextension {"dylib"} + defines { "WIN32" } + links {"Ws2_32","Winmm"} + end + if os.is("Linux") then + end + if os.is("MacOSX") then + end + + links {"enet"} + + files { + "../../examples/SharedMemory/PhysicsClientUDP.cpp", + "../../examples/SharedMemory/PhysicsClientUDP.h", + "../../examples/SharedMemory/PhysicsClientUDP_C_API.cpp", + "../../examples/SharedMemory/PhysicsClientUDP_C_API.h", + } + defines {"BT_ENABLE_ENET"} + end + + if not _OPTIONS["no-clsocket"] then + + includedirs {"../../examples/ThirdPartyLibs/clsocket/src"} + + if os.is("Windows") then + defines { "WIN32" } + links {"Ws2_32","Winmm"} + end + if os.is("Linux") then + defines {"_LINUX"} + end + if os.is("MacOSX") then + defines {"_DARWIN"} + end + + links {"clsocket"} + + files { + "../../examples/SharedMemory/PhysicsClientTCP.cpp", + "../../examples/SharedMemory/PhysicsClientTCP.h", + "../../examples/SharedMemory/PhysicsClientTCP_C_API.cpp", + "../../examples/SharedMemory/PhysicsClientTCP_C_API.h", + } + defines {"BT_ENABLE_CLSOCKET"} + end + + + files { + "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", + "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h", + "../../examples/SharedMemory/IKTrajectoryHelper.cpp", + "../../examples/SharedMemory/IKTrajectoryHelper.h", + "../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp", + "../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp", + "../../examples/OpenGLWindow/SimpleCamera.cpp", + "../../examples/OpenGLWindow/SimpleCamera.h", + "../../examples/TinyRenderer/geometry.cpp", + "../../examples/TinyRenderer/model.cpp", + "../../examples/TinyRenderer/tgaimage.cpp", + "../../examples/TinyRenderer/our_gl.cpp", + "../../examples/TinyRenderer/TinyRenderer.cpp", + "../../examples/SharedMemory/InProcessMemory.cpp", + "../../examples/SharedMemory/PhysicsClient.cpp", + "../../examples/SharedMemory/PhysicsClient.h", + "../../examples/SharedMemory/PhysicsServer.cpp", + "../../examples/SharedMemory/PhysicsServer.h", + "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsServerSharedMemory.h", + "../../examples/SharedMemory/PhysicsDirect.cpp", + "../../examples/SharedMemory/PhysicsDirect.h", + "../../examples/SharedMemory/PhysicsDirectC_API.cpp", + "../../examples/SharedMemory/PhysicsDirectC_API.h", + "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp", + "../../examples/SharedMemory/PhysicsServerCommandProcessor.h", + "../../examples/SharedMemory/b3PluginManager.cpp", + "../../examples/SharedMemory/b3PluginManager.h", + + "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsClientSharedMemory.h", + "../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp", + "../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h", + "../../examples/SharedMemory/PhysicsClientC_API.cpp", + + "../../examples/SharedMemory/PhysicsClientC_API.h", + "../../examples/SharedMemory/SharedMemoryPublic.h", + + "../../examples/SharedMemory/Win32SharedMemory.cpp", + "../../examples/SharedMemory/Win32SharedMemory.h", + "../../examples/SharedMemory/PosixSharedMemory.cpp", + "../../examples/SharedMemory/PosixSharedMemory.h", + + "../../examples/Utils/b3ResourcePath.cpp", + "../../examples/Utils/b3ResourcePath.h", + "../../examples/Utils/RobotLoggingUtil.cpp", + "../../examples/Utils/RobotLoggingUtil.h", + "../../examples/Utils/b3Clock.cpp", + "../../examples/Utils/b3ResourcePath.cpp", + "../../examples/Utils/b3ERPCFMHelper.hpp", + "../../examples/Utils/b3ReferenceFrameHelper.hpp", + "../../examples/Utils/ChromeTraceUtil.cpp", + + "../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp", + + "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp", + "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h", + + "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", + + "../../examples/ThirdPartyLibs/BussIK/Jacobian.cpp", + "../../examples/ThirdPartyLibs/BussIK/LinearR2.cpp", + "../../examples/ThirdPartyLibs/BussIK/LinearR3.cpp", + "../../examples/ThirdPartyLibs/BussIK/LinearR4.cpp", + "../../examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp", + "../../examples/ThirdPartyLibs/BussIK/Misc.cpp", + "../../examples/ThirdPartyLibs/BussIK/Node.cpp", + "../../examples/ThirdPartyLibs/BussIK/Tree.cpp", + "../../examples/ThirdPartyLibs/BussIK/VectorRn.cpp", + + "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", + "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", + "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", + "../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp", + "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", + "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", + "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", + "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", + "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", + "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + + "../../examples/MultiThreading/b3PosixThreadSupport.cpp", + "../../examples/MultiThreading/b3Win32ThreadSupport.cpp", + "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", + } + +if (_OPTIONS["enable_static_vr_plugin"]) then + files {"../../examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp"} +end + + + diff --git a/Extras/premake4.lua b/Extras/premake4.lua index be6f1bc19..416c019d4 100644 --- a/Extras/premake4.lua +++ b/Extras/premake4.lua @@ -7,3 +7,4 @@ include "Serialize/BulletFileLoader" include "Serialize/BulletWorldImporter" include "Serialize/BulletXmlWorldImporter" include "obj2sdf" +include "BulletRobotics" diff --git a/examples/RobotSimulator/premake4.lua b/examples/RobotSimulator/premake4.lua index 591cfea40..da007c232 100644 --- a/examples/RobotSimulator/premake4.lua +++ b/examples/RobotSimulator/premake4.lua @@ -1,73 +1,4 @@ -myfiles = -{ - - "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", - "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h", - "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp", - "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h", - "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp", - "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h", - "../../examples/SharedMemory/IKTrajectoryHelper.cpp", - "../../examples/SharedMemory/IKTrajectoryHelper.h", - "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp", - "../../examples/SharedMemory/InProcessMemory.cpp", - "../../examples/SharedMemory/PhysicsClient.cpp", - "../../examples/SharedMemory/PhysicsClient.h", - "../../examples/SharedMemory/PhysicsServer.cpp", - "../../examples/SharedMemory/PhysicsServer.h", - "../../examples/SharedMemory/PhysicsServerExample.cpp", - "../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp", - "../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp", - "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp", - "../../examples/SharedMemory/PhysicsServerSharedMemory.h", - "../../examples/SharedMemory/PhysicsDirect.cpp", - "../../examples/SharedMemory/PhysicsDirect.h", - "../../examples/SharedMemory/PhysicsDirectC_API.cpp", - "../../examples/SharedMemory/PhysicsDirectC_API.h", - "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp", - "../../examples/SharedMemory/PhysicsServerCommandProcessor.h", - "../../examples/SharedMemory/b3PluginManager.cpp", - - "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", - "../../examples/SharedMemory/PhysicsClientSharedMemory.h", - "../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp", - "../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h", - "../../examples/SharedMemory/PhysicsClientC_API.cpp", - "../../examples/SharedMemory/PhysicsClientC_API.h", - "../../examples/SharedMemory/Win32SharedMemory.cpp", - "../../examples/SharedMemory/Win32SharedMemory.h", - "../../examples/SharedMemory/PosixSharedMemory.cpp", - "../../examples/SharedMemory/PosixSharedMemory.h", - "../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp", - "../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp", - "../../examples/TinyRenderer/geometry.cpp", - "../../examples/TinyRenderer/model.cpp", - "../../examples/TinyRenderer/tgaimage.cpp", - "../../examples/TinyRenderer/our_gl.cpp", - "../../examples/TinyRenderer/TinyRenderer.cpp", - "../../examples/Utils/b3ResourcePath.cpp", - "../../examples/Utils/b3ResourcePath.h", - "../../examples/Utils/RobotLoggingUtil.cpp", - "../../examples/Utils/RobotLoggingUtil.h", - "../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp", - "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp", - "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h", - "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", - "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", - "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", - "../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp", - "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", - "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", - "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", - "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", - "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", - "../../examples/MultiThreading/b3PosixThreadSupport.cpp", - "../../examples/MultiThreading/b3Win32ThreadSupport.cpp", - "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", - "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", - "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", -} project ("App_RobotSimulator") @@ -78,9 +9,8 @@ project ("App_RobotSimulator") "../../examples/ThirdPartyLibs"} defines {"B3_USE_ROBOTSIM_GUI", "PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"} - hasCL = findOpenCL("clew") - links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK","Bullet3Common"} + links{"BulletRobotics", "BulletExampleBrowserLib", "gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"} initOpenGL() initGlew() @@ -96,17 +26,6 @@ project ("App_RobotSimulator") links{"Cocoa.framework"} end - if (hasCL) then - links { - "Bullet3OpenCL_clew", - "Bullet3Dynamics", - "Bullet3Collision", - "Bullet3Geometry", - "Bullet3Common", - } - end - - if not _OPTIONS["no-enet"] then @@ -200,7 +119,10 @@ if not _OPTIONS["no-enet"] then "b3RobotSimulatorClientAPI.h", "MinitaurSetup.cpp", "MinitaurSetup.h", - myfiles + "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp", + "../../examples/SharedMemory/PhysicsServerExample.cpp", + "../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp", + "../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp", } if (_OPTIONS["enable_static_vr_plugin"]) then @@ -225,7 +147,7 @@ project ("App_VRGloveHandSimulator") hasCL = findOpenCL("clew") - links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK","Bullet3Common"} + links{"BulletRobotics", "BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK","Bullet3Common"} initOpenGL() initGlew() @@ -288,7 +210,7 @@ project ("App_VRGloveHandSimulator") "VRGloveSimulatorMain.cpp", "b3RobotSimulatorClientAPI.cpp", "b3RobotSimulatorClientAPI.h", - myfiles + } if (_OPTIONS["enable_static_vr_plugin"]) then @@ -299,3 +221,64 @@ end initX11() end end + + +project ("App_HelloBulletRobotics") + + language "C++" + kind "ConsoleApp" + + links{"BulletRobotics","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"} + + includedirs { + ".", + "../../src", + "../../examples/SharedMemory", + "../ThirdPartyLibs", + } + +if not _OPTIONS["no-enet"] then + + includedirs {"../../examples/ThirdPartyLibs/enet/include"} + + if os.is("Windows") then + defines { "WIN32" } + links {"Ws2_32","Winmm"} + end + if os.is("Linux") then + end + if os.is("MacOSX") then + end + links {"enet"} + defines {"BT_ENABLE_ENET"} + end + + if not _OPTIONS["no-clsocket"] then + + includedirs {"../../examples/ThirdPartyLibs/clsocket/src"} + + if os.is("Windows") then + defines { "WIN32" } + links {"Ws2_32","Winmm"} + end + if os.is("Linux") then + defines {"_LINUX"} + end + if os.is("MacOSX") then + defines {"_DARWIN"} + end + + links {"clsocket"} + defines {"BT_ENABLE_CLSOCKET"} + end + + if os.is("MacOSX") then + links{"Cocoa.framework"} + end + + + files { + "HelloBulletRobotics.cpp" + } + + From 0cec85626f1436533461f9952bc42db43321178b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 9 Jun 2018 12:19:29 -0700 Subject: [PATCH 2/2] enable more unit tests in PyBullet. return Py_None if no user data found, to pass the test. enable m_deterministicOverlappingPairs by default. if m_deterministicOverlappingPairs, sort the collision pairs. --- .travis.yml | 2 + examples/pybullet/pybullet.c | 4 +- .../unittests/saveRestoreStateTest.py | 144 ++++++++++++++++++ .../BroadphaseCollision/btDispatcher.h | 2 +- .../btCollisionDispatcher.cpp | 10 +- 5 files changed, 158 insertions(+), 4 deletions(-) create mode 100644 examples/pybullet/unittests/saveRestoreStateTest.py diff --git a/.travis.yml b/.travis.yml index 5723a41f8..c93c1d2ce 100644 --- a/.travis.yml +++ b/.travis.yml @@ -19,6 +19,8 @@ script: - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then sudo pip3 install setuptools; fi - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then sudo python3 setup.py install; fi - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then python3 examples/pybullet/unittests/unittests.py --verbose; fi + - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then python3 examples/pybullet/unittests/userDataTest.py --verbose; fi + - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then python3 examples/pybullet/unittests/saveRestoreStateTest.py --verbose; fi - cmake . -DBUILD_PYBULLET=ON -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror - make -j8 - ctest -j8 --output-on-failure diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index a92bb5240..b2ef1027a 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -820,8 +820,8 @@ static PyObject* pybullet_getUserData(PyObject* self, PyObject* args, PyObject* if (!b3GetUserData(sm, bodyUniqueId, linkIndex, userDataId, &value)) { - PyErr_SetString(SpamError, "Cannot get user data"); - return NULL; + Py_INCREF(Py_None); + return Py_None; } if (value.m_type != USER_DATA_VALUE_TYPE_STRING) { diff --git a/examples/pybullet/unittests/saveRestoreStateTest.py b/examples/pybullet/unittests/saveRestoreStateTest.py new file mode 100644 index 000000000..da3480904 --- /dev/null +++ b/examples/pybullet/unittests/saveRestoreStateTest.py @@ -0,0 +1,144 @@ +import unittest +import pybullet as p +import math, time +import difflib,sys + +from utils import allclose, dot + +class TestPybulletSaveRestoreMethods(unittest.TestCase): + + def setupWorld(self): + numObjects = 50 + + + maximalCoordinates = False + + p.resetSimulation() + p.setPhysicsEngineParameter(deterministicOverlappingPairs=1) + p.loadURDF("planeMesh.urdf",useMaximalCoordinates=maximalCoordinates) + kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10], useMaximalCoordinates=maximalCoordinates) + for i in range (p.getNumJoints(kukaId)): + p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0) + for i in range (numObjects): + cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2]) + #p.changeDynamics(cube,-1,mass=100) + p.stepSimulation() + p.setGravity(0,0,-10) + + + def dumpStateToFile(self, file): + for i in range (p.getNumBodies()): + pos,orn = p.getBasePositionAndOrientation(i) + linVel,angVel = p.getBaseVelocity(i) + txtPos = "pos="+str(pos)+"\n" + txtOrn = "orn="+str(orn)+"\n" + txtLinVel = "linVel"+str(linVel)+"\n" + txtAngVel = "angVel"+str(angVel)+"\n" + file.write(txtPos) + file.write(txtOrn) + file.write(txtLinVel) + file.write(txtAngVel) + + def compareFiles(self, file1,file2): + diff = difflib.unified_diff( + file1.readlines(), + file2.readlines(), + fromfile='saveFile.txt', + tofile='restoreFile.txt', + ) + numDifferences = 0 + for line in diff: + numDifferences = numDifferences+1 + sys.stdout.write(line) + self.assertEqual(numDifferences,0) + + + def testSaveRestoreState(self): + numSteps = 500 + numSteps2 = 30 + + + verbose = 0 + self.setupWorld() + + + for i in range (numSteps): + p.stepSimulation() + p.saveBullet("state.bullet") + if verbose: + p.setInternalSimFlags(1) + p.stepSimulation() + if verbose: + p.setInternalSimFlags(0) + print("contact points=") + for q in p.getContactPoints(): + print(q) + + for i in range (numSteps2): + p.stepSimulation() + + + file = open("saveFile.txt","w") + self.dumpStateToFile(file) + file.close() + + ################################# + self.setupWorld() + + #both restore from file or from in-memory state should work + p.restoreState(fileName="state.bullet") + stateId = p.saveState() + + if verbose: + p.setInternalSimFlags(1) + p.stepSimulation() + if verbose: + p.setInternalSimFlags(0) + print("contact points restored=") + for q in p.getContactPoints(): + print(q) + for i in range (numSteps2): + p.stepSimulation() + + + file = open("restoreFile.txt","w") + self.dumpStateToFile(file) + file.close() + + p.restoreState(stateId) + if verbose: + p.setInternalSimFlags(1) + p.stepSimulation() + if verbose: + p.setInternalSimFlags(0) + print("contact points restored=") + for q in p.getContactPoints(): + print(q) + for i in range (numSteps2): + p.stepSimulation() + + + file = open("restoreFile2.txt","w") + self.dumpStateToFile(file) + file.close() + + file1 = open("saveFile.txt","r") + file2 = open("restoreFile.txt","r") + self.compareFiles(file1,file2) + file1.close() + file2.close() + + file1 = open("saveFile.txt","r") + file2 = open("restoreFile2.txt","r") + self.compareFiles(file1,file2) + file1.close() + file2.close() + + + +#while (p.getConnectionInfo()["isConnected"]): +# time.sleep(1) + +if __name__ == '__main__': + p.connect(p.DIRECT) + unittest.main() diff --git a/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/src/BulletCollision/BroadphaseCollision/btDispatcher.h index a0e4c1892..d7a97e8e0 100644 --- a/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -47,7 +47,7 @@ struct btDispatcherInfo m_allowedCcdPenetration(btScalar(0.04)), m_useConvexConservativeDistanceUtil(false), m_convexConservativeDistanceThreshold(0.0f), - m_deterministicOverlappingPairs(false) + m_deterministicOverlappingPairs(true) { } diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 5739a1ef0..af521ea9c 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -252,7 +252,15 @@ void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pa btCollisionPairCallback collisionCallback(dispatchInfo,this); - pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); + if (dispatchInfo.m_deterministicOverlappingPairs) + { + BT_PROFILE("sortOverlappingPairs"); + pairCache->sortOverlappingPairs(this); + } + { + BT_PROFILE("processAllOverlappingPairs"); + pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); + } //m_blockedForChanges = false;