diff --git a/Extras/Serialize/BulletFileLoader/bDNA.cpp b/Extras/Serialize/BulletFileLoader/bDNA.cpp index f30f7fec6..86d15cfdd 100644 --- a/Extras/Serialize/BulletFileLoader/bDNA.cpp +++ b/Extras/Serialize/BulletFileLoader/bDNA.cpp @@ -389,18 +389,10 @@ void bDNA::init(char *data, int len, bool swap) cp++; } + + + cp = btAlignPointer(cp,4); - { - nr= (long)cp; - //long mask=3; - nr= ((nr+3)&~3)-nr; - while (nr--) - { - cp++; - } - } - - /* TYPE (4 bytes) amount of types (int) @@ -426,17 +418,8 @@ void bDNA::init(char *data, int len, bool swap) cp++; } -{ - nr= (long)cp; - // long mask=3; - nr= ((nr+3)&~3)-nr; - while (nr--) - { - cp++; - } - } - - + cp = btAlignPointer(cp,4); + /* TLEN (4 bytes) (short) the lengths of types diff --git a/Extras/Serialize/BulletFileLoader/bFile.cpp b/Extras/Serialize/BulletFileLoader/bFile.cpp index 35135ba7a..cb8010dc2 100644 --- a/Extras/Serialize/BulletFileLoader/bFile.cpp +++ b/Extras/Serialize/BulletFileLoader/bFile.cpp @@ -460,15 +460,7 @@ void bFile::swapDNA(char* ptr) } - { - nr= (long)cp; - //long mask=3; - nr= ((nr+3)&~3)-nr; - while (nr--) - { - cp++; - } - } + cp = btAlignPointer(cp,4); /* @@ -497,16 +489,7 @@ void bFile::swapDNA(char* ptr) cp++; } -{ - nr= (long)cp; - // long mask=3; - nr= ((nr+3)&~3)-nr; - while (nr--) - { - cp++; - } - } - + cp = btAlignPointer(cp,4); /* TLEN (4 bytes) diff --git a/build_visual_studio.bat b/build_visual_studio.bat index de3b886c9..56ee9f610 100644 --- a/build_visual_studio.bat +++ b/build_visual_studio.bat @@ -2,7 +2,7 @@ rem premake4 --with-pe vs2010 rem premake4 --bullet2demos vs2010 cd build3 -premake4 --targetdir="../bin" vs2010 +premake4 --enable_openvr --targetdir="../bin" vs2010 rem premake4 --targetdir="../server2bin" vs2010 rem cd vs2010 rem rename 0_Bullet3Solution.sln 0_server.sln diff --git a/data/cube.mtl b/data/cube.mtl index 26b4b44e4..ffce2975d 100644 --- a/data/cube.mtl +++ b/data/cube.mtl @@ -11,4 +11,6 @@ newmtl cube Ke 0.0000 0.0000 0.0000 map_Ka cube.tga map_Kd cube.png + + diff --git a/data/cube.urdf b/data/cube.urdf new file mode 100644 index 000000000..679e106b5 --- /dev/null +++ b/data/cube.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/plane.mtl b/data/plane.mtl index 8ce4b8207..46c54e4be 100644 --- a/data/plane.mtl +++ b/data/plane.mtl @@ -1,11 +1,15 @@ -# Blender MTL File: 'None' -# Material Count: 1 - newmtl Material -Ns 96.078431 -Ka 0.000000 0.000000 0.000000 -Kd 0.640000 0.640000 0.640000 -Ks 0.500000 0.500000 0.500000 -Ni 1.000000 -d 1.000000 -illum 2 + Ns 10.0000 + Ni 1.5000 + d 1.0000 + Tr 0.0000 + Tf 1.0000 1.0000 1.0000 + illum 2 + Ka 0.0000 0.0000 0.0000 + Kd 0.5880 0.5880 0.5880 + Ks 0.0000 0.0000 0.0000 + Ke 0.0000 0.0000 0.0000 + map_Ka cube.tga + map_Kd checker_grid.jpg + + \ No newline at end of file diff --git a/data/plane.obj b/data/plane.obj index 64342328f..61e8e047c 100644 --- a/data/plane.obj +++ b/data/plane.obj @@ -2,11 +2,17 @@ # www.blender.org mtllib plane.mtl o Plane -v 1.000000 0.000000 -1.000000 -v 1.000000 0.000000 1.000000 -v -1.000000 0.000000 1.000000 -v -1.000000 0.000000 -1.000000 +v 5.000000 -5.000000 0.000000 +v 5.000000 5.000000 0.000000 +v -5.000000 5.000000 0.000000 +v -5.000000 -5.000000 0.000000 + +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 0.000000 0.000000 + usemtl Material s off -f 1 2 3 -f 1 3 4 +f 1/1 2/2 3/3 +f 1/1 3/3 4/4 diff --git a/data/r2d2.urdf b/data/r2d2.urdf index 9f7c750c0..091369db8 100644 --- a/data/r2d2.urdf +++ b/data/r2d2.urdf @@ -79,7 +79,7 @@ - + diff --git a/data/sphere2.urdf b/data/sphere2.urdf index fb0108b3d..cf7618d93 100644 --- a/data/sphere2.urdf +++ b/data/sphere2.urdf @@ -9,8 +9,11 @@ - + + + + @@ -19,55 +22,5 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/BasicDemo/BasicExample.cpp index b6365df36..b687c35b1 100644 --- a/examples/BasicDemo/BasicExample.cpp +++ b/examples/BasicDemo/BasicExample.cpp @@ -39,7 +39,7 @@ struct BasicExample : public CommonRigidBodyBase virtual void renderScene(); void resetCamera() { - float dist = 41; + float dist = 4; float pitch = 52; float yaw = 35; float targetPos[3]={0,0,0}; @@ -81,7 +81,7 @@ void BasicExample::initPhysics() //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance - btBoxShape* colShape = createBoxShape(btVector3(1,1,1)); + btBoxShape* colShape = createBoxShape(btVector3(.1,.1,.1)); //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); @@ -108,9 +108,9 @@ void BasicExample::initPhysics() for(int j = 0;jautogenerateGraphicsObjects(m_dynamicsWorld); + } diff --git a/examples/BasicDemo/CMakeLists.txt b/examples/BasicDemo/CMakeLists.txt index 2ad8f8ccf..f01a03a51 100644 --- a/examples/BasicDemo/CMakeLists.txt +++ b/examples/BasicDemo/CMakeLists.txt @@ -53,6 +53,7 @@ SET(AppBasicExampleGui_SRCS ../ExampleBrowser/OpenGLGuiHelper.cpp ../ExampleBrowser/GL_ShapeDrawer.cpp ../ExampleBrowser/CollisionShape2TriangleMesh.cpp + ../Utils/b3Clock.cpp ) #this define maps StandaloneExampleCreateFunc to the right 'CreateFunc' diff --git a/examples/BasicDemo/premake4.lua b/examples/BasicDemo/premake4.lua index dd88389e1..dd924249e 100644 --- a/examples/BasicDemo/premake4.lua +++ b/examples/BasicDemo/premake4.lua @@ -49,6 +49,8 @@ files { "../ExampleBrowser/OpenGLGuiHelper.cpp", "../ExampleBrowser/GL_ShapeDrawer.cpp", "../ExampleBrowser/CollisionShape2TriangleMesh.cpp", + "../Utils/b3Clock.cpp", + "../Utils/b3Clock.h", } if os.is("Linux") then initX11() end @@ -93,7 +95,9 @@ files { "../TinyRenderer/tgaimage.cpp", "../TinyRenderer/our_gl.cpp", "../TinyRenderer/TinyRenderer.cpp", - "../Utils/b3ResourcePath.cpp" + "../Utils/b3ResourcePath.cpp", + "../Utils/b3Clock.cpp", + "../Utils/b3Clock.h", } if os.is("Linux") then initX11() end @@ -132,7 +136,9 @@ files { "../TinyRenderer/tgaimage.cpp", "../TinyRenderer/our_gl.cpp", "../TinyRenderer/TinyRenderer.cpp", - "../Utils/b3ResourcePath.cpp" + "../Utils/b3ResourcePath.cpp", + "../Utils/b3Clock.cpp", + "../Utils/b3Clock.h", } @@ -179,6 +185,8 @@ files { "../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp", "../ThirdPartyLibs/openvr/samples/shared/pathtools.h", "../ThirdPartyLibs/openvr/samples/shared/Vectors.h", + "../Utils/b3Clock.cpp", + "../Utils/b3Clock.h", } diff --git a/examples/Benchmarks/BenchmarkDemo.cpp b/examples/Benchmarks/BenchmarkDemo.cpp index ca9f75786..f659d017c 100644 --- a/examples/Benchmarks/BenchmarkDemo.cpp +++ b/examples/Benchmarks/BenchmarkDemo.cpp @@ -1273,7 +1273,7 @@ void BenchmarkDemo::exitPhysics() -struct CommonExampleInterface* BenchmarkCreateFunc(struct CommonExampleOptions& options) +CommonExampleInterface* BenchmarkCreateFunc(struct CommonExampleOptions& options) { return new BenchmarkDemo(options.m_guiHelper,options.m_option); } \ No newline at end of file diff --git a/examples/CommonInterfaces/CommonCameraInterface.h b/examples/CommonInterfaces/CommonCameraInterface.h index 73147d760..9c1740788 100644 --- a/examples/CommonInterfaces/CommonCameraInterface.h +++ b/examples/CommonInterfaces/CommonCameraInterface.h @@ -6,7 +6,9 @@ struct CommonCameraInterface virtual void getCameraProjectionMatrix(float m[16])const = 0; virtual void getCameraViewMatrix(float m[16]) const = 0; - virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16])=0; + virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16])=0; + virtual void disableVRCamera()=0; + virtual bool isVRCamera() const =0; virtual void getCameraTargetPosition(float pos[3]) const = 0; virtual void getCameraPosition(float pos[3]) const = 0; diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index b9a4dfd26..a8ff41d4f 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -29,10 +29,11 @@ struct GUIHelperInterface virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld)=0; - virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) =0; - + virtual int registerTexture(const unsigned char* texels, int width, int height)=0; + virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) = 0; virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) =0; - + virtual void removeAllGraphicsInstances()=0; + virtual Common2dCanvasInterface* get2dCanvasInterface()=0; virtual CommonParameterInterface* getParameterInterface()=0; @@ -73,10 +74,11 @@ struct DummyGUIHelper : public GUIHelperInterface virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){} - virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) { return -1; } - - virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) { return -1;} - + virtual int registerTexture(const unsigned char* texels, int width, int height){return -1;} + virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId){return -1;} + virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) {return -1;} + virtual void removeAllGraphicsInstances(){} + virtual Common2dCanvasInterface* get2dCanvasInterface() { return 0; diff --git a/examples/CommonInterfaces/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h index e1d6170ef..0ac09975f 100644 --- a/examples/CommonInterfaces/CommonRenderInterface.h +++ b/examples/CommonInterfaces/CommonRenderInterface.h @@ -22,7 +22,7 @@ struct CommonRenderInterface virtual void init()=0; virtual void updateCamera(int upAxis)=0; virtual void removeAllInstances() = 0; - + virtual const CommonCameraInterface* getActiveCamera() const =0; virtual CommonCameraInterface* getActiveCamera()=0; virtual void setActiveCamera(CommonCameraInterface* cam)=0; @@ -52,6 +52,10 @@ struct CommonRenderInterface virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)=0; virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex)=0; virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex)=0; + virtual void writeSingleInstanceScaleToCPU(float* scale, int srcIndex)=0; + virtual void writeSingleInstanceScaleToCPU(double* scale, int srcIndex)=0; + + virtual int getTotalNumInstances() const = 0; virtual void writeTransforms()=0; virtual void enableBlend(bool blend)=0; diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 114adb61f..da15ba32b 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -114,7 +114,10 @@ SET(ExtendedTutorialsSources ../ExtendedTutorials/SimpleCloth.cpp ../ExtendedTutorials/Chain.cpp ../ExtendedTutorials/Bridge.cpp - ../ExtendedTutorials/RigidBodyFromObj.cpp + ../ExtendedTutorials/RigidBodyFromObj.cpp + ../ExtendedTutorials/InclinedPlane.cpp + ../ExtendedTutorials/InclinedPlane.h + ../ExtendedTutorials/NewtonsCradle.cpp ) SET(BulletExampleBrowser_SRCS @@ -158,6 +161,22 @@ SET(BulletExampleBrowser_SRCS ../Tutorial/Tutorial.h ../Tutorial/Dof6ConstraintTutorial.cpp ../Tutorial/Dof6ConstraintTutorial.h + ../ExtendedTutorials/SimpleBox.cpp + ../ExtendedTutorials/SimpleBox.h + ../ExtendedTutorials/MultipleBoxes.cpp + ../ExtendedTutorials/MultipleBoxes.h + ../ExtendedTutorials/SimpleCloth.cpp + ../ExtendedTutorials/SimpleCloth.h + ../ExtendedTutorials/SimpleJoint.cpp + ../ExtendedTutorials/SimpleJoint.h + ../ExtendedTutorials/NewtonsCradle.cpp + ../ExtendedTutorials/NewtonsCradle.h + ../ExtendedTutorials/NewtonsRopeCradle.cpp + ../ExtendedTutorials/NewtonsRopeCradle.h + ../ExtendedTutorials/InclinedPlane.cpp + ../ExtendedTutorials/InclinedPlane.h + ../ExtendedTutorials/MultiPendulum.cpp + ../ExtendedTutorials/MultiPendulum.h ../Collision/CollisionSdkC_Api.cpp ../Collision/CollisionSdkC_Api.h ../Collision/CollisionTutorialBullet2.cpp @@ -189,7 +208,10 @@ SET(BulletExampleBrowser_SRCS ../RenderingExamples/TimeSeriesCanvas.h ../RenderingExamples/TimeSeriesFontData.cpp ../RenderingExamples/TimeSeriesFontData.h - + ../RoboticsLearning/b3RobotSimAPI.cpp + ../RoboticsLearning/b3RobotSimAPI.h + ../RoboticsLearning/R2D2GraspExample.cpp + ../RoboticsLearning/R2D2GraspExample.h ../RenderingExamples/CoordinateSystemDemo.cpp ../RenderingExamples/CoordinateSystemDemo.h ../RenderingExamples/RaytracerSetup.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index b6004aeef..16185efe4 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -45,6 +45,7 @@ #include "../Tutorial/Dof6ConstraintTutorial.h" #include "../MultiThreading/MultiThreadingExample.h" #include "../InverseDynamics/InverseDynamicsExample.h" +#include "../RoboticsLearning/R2D2GraspExample.h" #ifdef ENABLE_LUA #include "../LuaDemo/LuaPhysicsSetup.h" @@ -57,7 +58,7 @@ #endif #endif //B3_USE_CLEW -//Extended Tutorial Includes Added by Mobeen +//Extended Tutorial Includes Added by Mobeen and Benelot #include "../ExtendedTutorials/SimpleBox.h" #include "../ExtendedTutorials/MultipleBoxes.h" #include "../ExtendedTutorials/SimpleJoint.h" @@ -65,6 +66,10 @@ #include "../ExtendedTutorials/Chain.h" #include "../ExtendedTutorials/Bridge.h" #include "../ExtendedTutorials/RigidBodyFromObj.h" +#include "../ExtendedTutorials/InclinedPlane.h" +#include "../ExtendedTutorials/NewtonsCradle.h" +#include "../ExtendedTutorials/NewtonsRopeCradle.h" +#include "../ExtendedTutorials/MultiPendulum.h" struct ExampleEntry { @@ -90,11 +95,11 @@ struct ExampleEntry static ExampleEntry gDefaultExamples[]= { + ExampleEntry(0,"API"), ExampleEntry(1,"Basic Example","Create some rigid bodies using box collision shapes. This is a good example to familiarize with the basic initialization of Bullet. The Basic Example can also be compiled without graphical user interface, as a console application. Press W for wireframe, A to show AABBs, I to suspend/restart physics simulation. Press D to toggle auto-deactivation of the simulation. ", BasicExampleCreateFunc), - ExampleEntry(1,"Rolling Friction", "Damping is often not good enough to keep rounded objects from rolling down a sloped surface. Instead, you can set the rolling friction of a rigid body. Generally it is best to leave the rolling friction to zero, to avoid artifacts.", RollingFrictionCreateFunc), ExampleEntry(1,"Constraints","Show the use of the various constraints in Bullet. Press the L key to visualize the constraint limits. Press the C key to visualize the constraint frames.", @@ -114,8 +119,6 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",RigidBodySoftContactCreateFunc), - - ExampleEntry(0,"MultiBody"), ExampleEntry(1,"MultiDofCreateFunc","Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc), ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc), @@ -241,11 +244,12 @@ static ExampleEntry gDefaultExamples[]= PhysicsServerCreateFunc,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING), ExampleEntry(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.", PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG), - ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc), ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT), - + ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP), + ExampleEntry(1,"URDF Compliant Contact","Experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT), + #ifdef ENABLE_LUA @@ -273,11 +277,16 @@ static ExampleEntry gDefaultExamples[]= //Extended Tutorials Added by Mobeen ExampleEntry(0,"Extended Tutorials"), ExampleEntry(1,"Simple Box", "Simplest possible demo creating a single box rigid body that falls under gravity", ET_SimpleBoxCreateFunc), - ExampleEntry(1,"Multiple Boxes", "Adding multiple box rigid bodies that fall under gravity", ET_MultipleBoxesCreateFunc), - ExampleEntry(1,"Simple Joint", "Creating a single distance constraint between two box rigid bodies", ET_SimpleJointCreateFunc), - ExampleEntry(1,"Simple Cloth", "Creating a simple piece of cloth", ET_SimpleClothCreateFunc), - ExampleEntry(1,"Simple Chain", "Creating a simple chain using a pair of point2point/distance constraints. You may click and drag any box to see the chain respond.", ET_ChainCreateFunc), - ExampleEntry(1,"Simple Bridge", "Creating a simple bridge using a pair of point2point/distance constraints. You may click and drag any plank to see the bridge respond.", ET_BridgeCreateFunc), + ExampleEntry(1,"Multiple Boxes", "Add multiple box rigid bodies that fall under gravity", ET_MultipleBoxesCreateFunc), + ExampleEntry(1,"Simple Joint", "Create a single distance constraint between two box rigid bodies", ET_SimpleJointCreateFunc), + ExampleEntry(1,"Simple Cloth", "Create a simple piece of cloth", ET_SimpleClothCreateFunc), + ExampleEntry(1,"Simple Chain", "Create a simple chain using a pair of point2point/distance constraints. You may click and drag any box to see the chain respond.", ET_ChainCreateFunc), + ExampleEntry(1,"Simple Bridge", "Create a simple bridge using a pair of point2point/distance constraints. You may click and drag any plank to see the bridge respond.", ET_BridgeCreateFunc), + ExampleEntry(1,"Inclined Plane", "Create an inclined plane to show restitution and different types of friction. Use the sliders to vary restitution and friction and press space to reset the scene.", ET_InclinedPlaneCreateFunc), + ExampleEntry(1,"Newton's Cradle", "Create a Newton's Cradle using a pair of point2point/slider constraints. Press 1/2 to lengthen/shorten the pendula, press 3 to displace pendula. Use the sliders to select the number (reset simulation), length and restitution of pendula, the number of displaced pendula and apply the displacement force.", ET_NewtonsCradleCreateFunc), + ExampleEntry(1,"Newton's Rope Cradle", "Create a Newton's Cradle using ropes. Press 3 to displace pendula. Use the sliders to select the number (reset simulation), length and restitution of pendula and the number of displaced pendula and apply the displacement force.",ET_NewtonsRopeCradleCreateFunc), + ExampleEntry(1,"Multi-Pendulum", "Create a Multi-Pendulum using point2point/slider constraints. Press 1/2 to lengthen/shorten the pendula, press 3 to displace pendula. Use the sliders to select the number (reset simulation), length and restitution of pendula, the number of displaced pendula and apply the displacement force.",ET_MultiPendulumCreateFunc), + //todo: create a category/tutorial about advanced topics, such as optimizations, using different collision detection algorithm, different constraint solvers etc. //ExampleEntry(0,"Advanced"), diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp index b7254db71..e562b5d52 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp +++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp @@ -226,7 +226,7 @@ enum TestExampleBrowserCommunicationEnums void ExampleBrowserThreadFunc(void* userPtr,void* lsMemory) { - printf("thread started\n"); + printf("ExampleBrowserThreadFunc started\n"); ExampleBrowserThreadLocalStorage* localStorage = (ExampleBrowserThreadLocalStorage*) lsMemory; @@ -369,7 +369,7 @@ void btShutDownExampleBrowser(btInProcessExampleBrowserInternalData* data) } }; - printf("stopping threads\n"); + printf("btShutDownExampleBrowser stopping threads\n"); delete data->m_threadSupport; delete data->m_sharedMem; delete data; diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 13933af95..1ce196b57 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -399,7 +399,6 @@ static void loadCurrentSettings(const char* startFileName, b3CommandLineArgs& ar FILE* f = fopen(startFileName,"r"); if (f) { - int result; char oneline[1024]; char* argv[] = {0,&oneline[0]}; diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index 23c225677..0af5c8577 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -199,9 +199,16 @@ void OpenGLGuiHelper::createCollisionObjectGraphicsObject(btCollisionObject* bod } } -int OpenGLGuiHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) +int OpenGLGuiHelper::registerTexture(const unsigned char* texels, int width, int height) { - int shapeId = m_data->m_glApp->m_renderer->registerShape(vertices, numvertices,indices,numIndices); + int textureId = m_data->m_glApp->m_renderer->registerTexture(texels,width,height); + return textureId; +} + + +int OpenGLGuiHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) +{ + int shapeId = m_data->m_glApp->m_renderer->registerShape(vertices, numvertices,indices,numIndices,primitiveType, textureId); return shapeId; } @@ -210,6 +217,10 @@ int OpenGLGuiHelper::registerGraphicsInstance(int shapeIndex, const float* posit return m_data->m_glApp->m_renderer->registerGraphicsInstance(shapeIndex,position,quaternion,color,scaling); } +void OpenGLGuiHelper::removeAllGraphicsInstances() +{ + m_data->m_glApp->m_renderer->removeAllInstances(); +} void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* collisionShape) { @@ -247,7 +258,7 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli if (gfxVertices.size() && indices.size()) { - int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size()); + int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size(),B3_GL_TRIANGLES,-1); collisionShape->setUserIndex(shapeId); } diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index 0184d6ea0..707b325ae 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -20,12 +20,11 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void createCollisionObjectGraphicsObject(btCollisionObject* body, const btVector3& color); - virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices); - + virtual int registerTexture(const unsigned char* texels, int width, int height); + virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId); virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling); - + virtual void removeAllGraphicsInstances(); - virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape); virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld); diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 82c63e64d..7d11eaed9 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -86,14 +86,9 @@ project "App_BulletExampleBrowser" "../InverseDynamics/InverseDynamicsExample.h", "../BasicDemo/BasicExample.*", "../Tutorial/*", - "../ExtendedTutorials/SimpleBox.cpp", - "../ExtendedTutorials/MultipleBoxes.cpp", - "../ExtendedTutorials/SimpleJoint.cpp", - "../ExtendedTutorials/SimpleCloth.cpp", - "../ExtendedTutorials/Chain.cpp", - "../ExtendedTutorials/Bridge.cpp", - "../ExtendedTutorials/RigidBodyFromObj.cpp", + "../ExtendedTutorials/*", "../Collision/*", + "../RoboticsLearning/*", "../Collision/Internal/*", "../Benchmarks/*", "../CommonInterfaces/*", diff --git a/examples/ExtendedTutorials/InclinedPlane.cpp b/examples/ExtendedTutorials/InclinedPlane.cpp new file mode 100644 index 000000000..dceafdbe8 --- /dev/null +++ b/examples/ExtendedTutorials/InclinedPlane.cpp @@ -0,0 +1,372 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#include "InclinedPlane.h" + +#include "btBulletDynamicsCommon.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonParameterInterface.h" + +static btScalar gTilt = 20.0f/180.0f*SIMD_PI; // tilt the ramp 20 degrees + +static btScalar gRampFriction = 1; // set ramp friction to 1 + +static btScalar gRampRestitution = 0; // set ramp restitution to 0 (no restitution) + +static btScalar gBoxFriction = 1; // set box friction to 1 + +static btScalar gBoxRestitution = 0; // set box restitution to 0 + +static btScalar gSphereFriction = 1; // set sphere friction to 1 + +static btScalar gSphereRollingFriction = 1; // set sphere rolling friction to 1 + +static btScalar gSphereRestitution = 0; // set sphere restitution to 0 + +// handles for changes +static btRigidBody* ramp = NULL; +static btRigidBody* gBox = NULL; +static btRigidBody* gSphere = NULL; + +struct InclinedPlaneExample : public CommonRigidBodyBase +{ + InclinedPlaneExample(struct GUIHelperInterface* helper) + :CommonRigidBodyBase(helper) + { + } + virtual ~InclinedPlaneExample(){} + virtual void initPhysics(); + virtual void resetScene(); + virtual void renderScene(); + virtual void stepSimulation(float deltaTime); + virtual bool keyboardCallback(int key, int state); + void resetCamera() + { + float dist = 41; + float pitch = 52; + float yaw = 35; + float targetPos[3]={0,0.46,0}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } + + + +}; + +void onBoxFrictionChanged(float friction); + +void onBoxRestitutionChanged(float restitution); + +void onSphereFrictionChanged(float friction); + +void onSphereRestitutionChanged(float restitution); + +void onRampInclinationChanged(float inclination); + +void onRampFrictionChanged(float friction); + +void onRampRestitutionChanged(float restitution); + +void InclinedPlaneExample::initPhysics() +{ + + { // create slider to change the ramp tilt + SliderParams slider("Ramp Tilt",&gTilt); + slider.m_minVal=0; + slider.m_maxVal=SIMD_PI/2.0f; + slider.m_clampToNotches = false; + slider.m_callback = onRampInclinationChanged; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { // create slider to change the ramp friction + SliderParams slider("Ramp Friction",&gRampFriction); + slider.m_minVal=0; + slider.m_maxVal=10; + slider.m_clampToNotches = false; + slider.m_callback = onRampFrictionChanged; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { // create slider to change the ramp restitution + SliderParams slider("Ramp Restitution",&gRampRestitution); + slider.m_minVal=0; + slider.m_maxVal=1; + slider.m_clampToNotches = false; + slider.m_callback = onRampRestitutionChanged; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { // create slider to change the box friction + SliderParams slider("Box Friction",&gBoxFriction); + slider.m_minVal=0; + slider.m_maxVal=10; + slider.m_clampToNotches = false; + slider.m_callback = onBoxFrictionChanged; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { // create slider to change the box restitution + SliderParams slider("Box Restitution",&gBoxRestitution); + slider.m_minVal=0; + slider.m_maxVal=1; + slider.m_clampToNotches = false; + slider.m_callback = onBoxRestitutionChanged; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { // create slider to change the sphere friction + SliderParams slider("Sphere Friction",&gSphereFriction); + slider.m_minVal=0; + slider.m_maxVal=10; + slider.m_clampToNotches = false; + slider.m_callback = onSphereFrictionChanged; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { // create slider to change the sphere rolling friction + SliderParams slider("Sphere Rolling Friction",&gSphereRollingFriction); + slider.m_minVal=0; + slider.m_maxVal=10; + slider.m_clampToNotches = false; + slider.m_callback = onSphereRestitutionChanged; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { // create slider to change the sphere restitution + SliderParams slider("Sphere Restitution",&gSphereRestitution); + slider.m_minVal=0; + slider.m_maxVal=1; + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + m_guiHelper->setUpAxis(1); // set Y axis as up axis + + createEmptyDynamicsWorld(); + + // create debug drawer + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + if (m_dynamicsWorld->getDebugDrawer()) + m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); + + + { // create a static ground + btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-50,0)); + + btScalar mass(0.); + createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); + } + + { //create a static inclined plane + btBoxShape* inclinedPlaneShape = createBoxShape(btVector3(btScalar(20.),btScalar(1.),btScalar(10.))); + m_collisionShapes.push_back(inclinedPlaneShape); + + btTransform startTransform; + startTransform.setIdentity(); + + // position the inclined plane above ground + startTransform.setOrigin(btVector3( + btScalar(0), + btScalar(15), + btScalar(0))); + + btQuaternion incline; + incline.setRotation(btVector3(0,0,1),gTilt); + startTransform.setRotation(incline); + + btScalar mass(0.); + ramp = createRigidBody(mass,startTransform,inclinedPlaneShape); + ramp->setFriction(gRampFriction); + ramp->setRestitution(gRampRestitution); + } + + + { //create a cube above the inclined plane + btBoxShape* boxShape = createBoxShape(btVector3(1,1,1)); + + m_collisionShapes.push_back(boxShape); + + btTransform startTransform; + startTransform.setIdentity(); + + btScalar boxMass(1.f); + + startTransform.setOrigin( + btVector3(btScalar(0), btScalar(20), btScalar(2))); + + gBox = createRigidBody(boxMass, startTransform, boxShape); + gBox->forceActivationState(DISABLE_DEACTIVATION); // to prevent the box on the ramp from disabling + gBox->setFriction(gBoxFriction); + gBox->setRestitution(gBoxRestitution); + } + + { //create a sphere above the inclined plane + btSphereShape* sphereShape = new btSphereShape(btScalar(1)); + + m_collisionShapes.push_back(sphereShape); + + btTransform startTransform; + startTransform.setIdentity(); + + btScalar sphereMass(1.f); + + startTransform.setOrigin( + btVector3(btScalar(0), btScalar(20), btScalar(4))); + + gSphere = createRigidBody(sphereMass, startTransform, sphereShape); + gSphere->forceActivationState(DISABLE_DEACTIVATION); // to prevent the sphere on the ramp from disabling + gSphere->setFriction(gSphereFriction); + gSphere->setRestitution(gSphereRestitution); + gSphere->setRollingFriction(gSphereRollingFriction); + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void InclinedPlaneExample::resetScene() { + { //reset a cube above the inclined plane + + btTransform startTransform; + startTransform.setIdentity(); + + startTransform.setOrigin( + btVector3(btScalar(0), btScalar(20), btScalar(2))); + + gBox->setWorldTransform(startTransform); + btVector3 zero(0, 0, 0); + gBox->setAngularVelocity(zero); + gBox->setLinearVelocity(zero); + gBox->clearForces(); + } + + { //reset a sphere above the inclined plane + btTransform startTransform; + startTransform.setIdentity(); + + startTransform.setOrigin( + btVector3(btScalar(0), btScalar(20), btScalar(4))); + + gSphere->setWorldTransform(startTransform); + btVector3 zero(0, 0, 0); + gSphere->setAngularVelocity(zero); + gSphere->setLinearVelocity(zero); + gSphere->clearForces(); + } +} + +void InclinedPlaneExample::stepSimulation(float deltaTime) +{ + if (m_dynamicsWorld) + { + m_dynamicsWorld->stepSimulation(deltaTime); + } + +} + + +void InclinedPlaneExample::renderScene() +{ + CommonRigidBodyBase::renderScene(); +} + +bool InclinedPlaneExample::keyboardCallback(int key, int state) { +// b3Printf("Key pressed: %d in state %d \n",key,state); + + switch (key) { + case 32 /*ASCII for space*/: { + resetScene(); + break; + } + } + + return false; +} + + +// GUI parameter modifiers +void onBoxFrictionChanged(float friction){ + if(gBox){ + gBox->setFriction(friction); +// b3Printf("Friction of box changed to %f",friction ); + } +} + +void onBoxRestitutionChanged(float restitution){ + if(gBox){ + gBox->setRestitution(restitution); + //b3Printf("Restitution of box changed to %f",restitution); + } +} + +void onSphereFrictionChanged(float friction){ + if(gSphere){ + gSphere->setFriction(friction); + //b3Printf("Friction of sphere changed to %f",friction ); + } +} + +void onSphereRestitutionChanged(float restitution){ + if(gSphere){ + gSphere->setRestitution(restitution); + //b3Printf("Restitution of sphere changed to %f",restitution); + } +} + +void onRampInclinationChanged(float inclination){ + if(ramp){ + btTransform startTransform; + startTransform.setIdentity(); + + // position the inclined plane above ground + startTransform.setOrigin( + btVector3(btScalar(0), btScalar(15), btScalar(0))); + + btQuaternion incline; + incline.setRotation(btVector3(0,0,1),gTilt); + startTransform.setRotation(incline); + ramp->setWorldTransform(startTransform); + //b3Printf("Inclination of ramp changed to %f",inclination ); + } +} + +void onRampFrictionChanged(float friction){ + if(ramp){ + ramp->setFriction(friction); + //b3Printf("Friction of ramp changed to %f \n",friction ); + } +} + +void onRampRestitutionChanged(float restitution){ + if(ramp){ + ramp->setRestitution(restitution); + //b3Printf("Restitution of ramp changed to %f \n",restitution); + } +} + + +CommonExampleInterface* ET_InclinedPlaneCreateFunc(CommonExampleOptions& options) +{ + return new InclinedPlaneExample(options.m_guiHelper); +} diff --git a/examples/ExtendedTutorials/InclinedPlane.h b/examples/ExtendedTutorials/InclinedPlane.h new file mode 100644 index 000000000..23ea92dfd --- /dev/null +++ b/examples/ExtendedTutorials/InclinedPlane.h @@ -0,0 +1,22 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef ET_INCLINED_PLANE_EXAMPLE_H +#define ET_INCLINED_PLANE_EXAMPLE_H + +class CommonExampleInterface* ET_InclinedPlaneCreateFunc(struct CommonExampleOptions& options); + + +#endif //ET_INCLINED_PLANE_EXAMPLE_H diff --git a/examples/ExtendedTutorials/MultiPendulum.cpp b/examples/ExtendedTutorials/MultiPendulum.cpp new file mode 100644 index 000000000..038aab3ec --- /dev/null +++ b/examples/ExtendedTutorials/MultiPendulum.cpp @@ -0,0 +1,435 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2015 Google Inc. http://bulletphysics.org + + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include "MultiPendulum.h" + +#include // TODO: Should I use another data structure? +#include + +#include "btBulletDynamicsCommon.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonParameterInterface.h" + +static btScalar gPendulaQty = 2; //TODO: This would actually be an Integer, but the Slider does not like integers, so I floor it when changed + +static btScalar gDisplacedPendula = 1; //TODO: This is an int as well + +static btScalar gPendulaRestitution = 1; // Default pendulum restitution is 1 to restore all force + +static btScalar gSphereRadius = 1; // The sphere radius + +static btScalar gCurrentPendulumLength = 8; + +static btScalar gInitialPendulumLength = 8; // Default pendulum length (distance between two spheres) + +static btScalar gDisplacementForce = 30; // The default force with which we move the pendulum + +static btScalar gForceScalar = 0; // default force scalar to apply a displacement + +struct MultiPendulumExample: public CommonRigidBodyBase { + MultiPendulumExample(struct GUIHelperInterface* helper) : + CommonRigidBodyBase(helper) { + } + + virtual ~MultiPendulumExample() { + } + + virtual void initPhysics(); // build a multi pendulum + virtual void renderScene(); // render the scene to screen + virtual void createMultiPendulum(btSphereShape* colShape, btScalar pendulaQty, const btVector3& position, btScalar length, btScalar mass); // create a multi pendulum at the indicated x and y position, the specified number of pendula formed into a chain, each with indicated length and mass + virtual void changePendulaLength(btScalar length); // change the pendulum length + virtual void changePendulaRestitution(btScalar restitution); // change the pendula restitution + virtual void stepSimulation(float deltaTime); // step the simulation + virtual bool keyboardCallback(int key, int state); // handle keyboard callbacks + virtual void applyPendulumForce(btScalar pendulumForce); + void resetCamera() { + float dist = 41; + float pitch = 52; + float yaw = 35; + float targetPos[3] = { 0, 0.46, 0 }; + m_guiHelper->resetCamera(dist, pitch, yaw, targetPos[0], targetPos[1], + targetPos[2]); + } + + std::vector constraints; // keep a handle to the slider constraints + std::vector pendula; // keep a handle to the pendula +}; + +static MultiPendulumExample* mex = NULL; // Handle to the example to access it via functions. Do not use this in your simulation! + +void onMultiPendulaLengthChanged(float pendulaLength); // Change the pendula length + +void onMultiPendulaRestitutionChanged(float pendulaRestitution); // change the pendula restitution + +void floorMSliderValue(float notUsed); // floor the slider values which should be integers + +void applyMForceWithForceScalar(float forceScalar); + +void MultiPendulumExample::initPhysics() { // Setup your physics scene + + { // create a slider to change the number of pendula + SliderParams slider("Number of Pendula", &gPendulaQty); + slider.m_minVal = 1; + slider.m_maxVal = 50; + slider.m_callback = floorMSliderValue; // hack to get integer values + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to change the number of displaced pendula + SliderParams slider("Number of Displaced Pendula", &gDisplacedPendula); + slider.m_minVal = 0; + slider.m_maxVal = 49; + slider.m_callback = floorMSliderValue; // hack to get integer values + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to change the pendula restitution + SliderParams slider("Pendula Restitution", &gPendulaRestitution); + slider.m_minVal = 0; + slider.m_maxVal = 1; + slider.m_clampToNotches = false; + slider.m_callback = onMultiPendulaRestitutionChanged; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to change the pendulum length + SliderParams slider("Pendula Length", &gCurrentPendulumLength); + slider.m_minVal = 0; + slider.m_maxVal = 49; + slider.m_clampToNotches = false; + slider.m_callback = onMultiPendulaLengthChanged; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to change the force to displace the lowest pendulum + SliderParams slider("Displacement force", &gDisplacementForce); + slider.m_minVal = 0.1; + slider.m_maxVal = 200; + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to apply the force by slider + SliderParams slider("Apply displacement force", &gForceScalar); + slider.m_minVal = -1; + slider.m_maxVal = 1; + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + m_guiHelper->setUpAxis(1); + + createEmptyDynamicsWorld(); + + // create a debug drawer + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + if (m_dynamicsWorld->getDebugDrawer()) + m_dynamicsWorld->getDebugDrawer()->setDebugMode( + btIDebugDraw::DBG_DrawWireframe + + btIDebugDraw::DBG_DrawContactPoints + + btIDebugDraw::DBG_DrawConstraints + + btIDebugDraw::DBG_DrawConstraintLimits); + + { // create the multipendulum starting at the indicated position below and where each pendulum has the following mass + btScalar pendulumMass(1.f); + + btVector3 position(0.0f,15.0f,0.0f); // initial top-most pendulum position + + // Re-using the same collision is better for memory usage and performance + btSphereShape* pendulumShape = new btSphereShape(gSphereRadius); + m_collisionShapes.push_back(pendulumShape); + + // create multi-pendulum + createMultiPendulum(pendulumShape, floor(gPendulaQty), position, + gInitialPendulumLength, pendulumMass); + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void MultiPendulumExample::stepSimulation(float deltaTime) { + + applyMForceWithForceScalar(gForceScalar); // apply force defined by apply force slider + + if (m_dynamicsWorld) { + m_dynamicsWorld->stepSimulation(deltaTime); + } +} + +void MultiPendulumExample::createMultiPendulum(btSphereShape* colShape, + btScalar pendulaQty, const btVector3& position, + btScalar length, btScalar mass) { + + // The multi-pendulum looks like this (names when built): + //..........0......./.......1...../.......2......./..etc...:pendulum build iterations + // O parentSphere + // | + // O childSphere / parentSphere + // | + // O ............./ childSphere / parentSphere + // | + // O .........................../ childSphere + // etc. + + //create the top element of the pendulum + btTransform startTransform; + startTransform.setIdentity(); + + // position the top sphere + startTransform.setOrigin(position); + + startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation + + btRigidBody* topSphere = createRigidBody(mass, startTransform, colShape); + + // disable the deactivation when object does not move anymore + topSphere->setActivationState(DISABLE_DEACTIVATION); + + //make top sphere position "fixed" in the world by attaching it to a the world with a point to point constraint + // The pivot is defined in the reference frame of topSphere, so the attachment should be exactly at the center of topSphere + btVector3 constraintPivot(0.0f, 0.0f, 0.0f); + btPoint2PointConstraint* p2pconst = new btPoint2PointConstraint( + *topSphere, constraintPivot); + + p2pconst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing + + // add the constraint to the world + m_dynamicsWorld->addConstraint(p2pconst, true); + + btRigidBody* parentSphere = topSphere; // set the top sphere as the parent sphere for the next sphere to be created + + for (int i = 0; i < pendulaQty; i++) { // produce the number of pendula + + // create joint element to make the pendulum rotate it + + // position the joint sphere at the same position as the top sphere + startTransform.setOrigin(position - btVector3(0,length*(i),0)); + + startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation + + btRigidBody* jointSphere = createRigidBody(mass, startTransform, + colShape); + jointSphere->setFriction(0); // we do not need friction here + + // disable the deactivation when object does not move anymore + jointSphere->setActivationState(DISABLE_DEACTIVATION); + + //create constraint between parentSphere and jointSphere + // this is represented by the constraint pivot in the local frames of reference of both constrained spheres + btTransform constraintPivotInParentSphereRF, constraintPivotInJointSphereRF; + + constraintPivotInParentSphereRF.setIdentity(); + constraintPivotInJointSphereRF.setIdentity(); + + // the orientation of a point-to-point constraint does not matter, as is has no rotational limits + + //Obtain the position of parentSphere in local reference frame of the jointSphere (the pivot is therefore in the center of parentSphere) + btVector3 parentSphereInJointSphereRF = + (jointSphere->getWorldTransform().inverse()( + parentSphere->getWorldTransform().getOrigin())); + constraintPivotInJointSphereRF.setOrigin(parentSphereInJointSphereRF); + + btPoint2PointConstraint* p2pconst = new btPoint2PointConstraint( + *parentSphere,*jointSphere,constraintPivotInParentSphereRF.getOrigin(), constraintPivotInJointSphereRF.getOrigin()); + + p2pconst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing + + // add the constraint to the world + m_dynamicsWorld->addConstraint(p2pconst, true); + + // create a slider constraint to change the length of the pendula while it swings + + startTransform.setIdentity(); // reset start transform + + // position the child sphere below the joint sphere + startTransform.setOrigin(position - btVector3(0,length*(i+1),0)); + + startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation + + btRigidBody* childSphere = createRigidBody(mass, startTransform, + colShape); + childSphere->setFriction(0); // we do not need friction here + pendula.push_back(childSphere); + + // disable the deactivation when object does not move anymore + childSphere->setActivationState(DISABLE_DEACTIVATION); + + //create slider constraint between jointSphere and childSphere + // this is represented by the constraint pivot in the local frames of reference of both constrained spheres + // furthermore we need to rotate the constraint appropriately to orient it correctly in space + btTransform constraintPivotInChildSphereRF; + + constraintPivotInJointSphereRF.setIdentity(); + constraintPivotInChildSphereRF.setIdentity(); + + // the orientation of a point-to-point constraint does not matter, as is has no rotational limits + + //Obtain the position of jointSphere in local reference frame of the childSphere (the pivot is therefore in the center of jointSphere) + btVector3 jointSphereInChildSphereRF = + (childSphere->getWorldTransform().inverse()( + jointSphere->getWorldTransform().getOrigin())); + constraintPivotInChildSphereRF.setOrigin(jointSphereInChildSphereRF); + + // the slider constraint is x aligned per default, but we want it to be y aligned, therefore we rotate it + btQuaternion qt; + qt.setEuler(0, 0, -SIMD_HALF_PI); + constraintPivotInJointSphereRF.setRotation(qt); //we use Y like up Axis + constraintPivotInChildSphereRF.setRotation(qt); //we use Y like up Axis + + btSliderConstraint* sliderConst = new btSliderConstraint(*jointSphere, + *childSphere, constraintPivotInJointSphereRF, constraintPivotInChildSphereRF, true); + + sliderConst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing + + // set limits + // the initial setup of the constraint defines the origins of the limit dimensions, + // therefore we set both limits directly to the current position of the parentSphere + sliderConst->setLowerLinLimit(btScalar(0)); + sliderConst->setUpperLinLimit(btScalar(0)); + sliderConst->setLowerAngLimit(btScalar(0)); + sliderConst->setUpperAngLimit(btScalar(0)); + constraints.push_back(sliderConst); + + // add the constraint to the world + m_dynamicsWorld->addConstraint(sliderConst, true); + parentSphere = childSphere; + } +} + +void MultiPendulumExample::changePendulaLength(btScalar length) { + btScalar lowerLimit = -gInitialPendulumLength; + for (std::vector::iterator sit = constraints.begin(); + sit != constraints.end(); sit++) { + btAssert((*sit) && "Null constraint"); + + // if the pendulum is being shortened beyond it's own length, we don't let the lower sphere to go past the upper one + if (lowerLimit <= length) { + (*sit)->setLowerLinLimit(length + lowerLimit); + (*sit)->setUpperLinLimit(length + lowerLimit); + } + } +} + +void MultiPendulumExample::changePendulaRestitution(btScalar restitution) { + for (std::vector::iterator rit = pendula.begin(); + rit != pendula.end(); rit++) { + btAssert((*rit) && "Null constraint"); + + (*rit)->setRestitution(restitution); + } +} + +void MultiPendulumExample::renderScene() { + CommonRigidBodyBase::renderScene(); +} + +bool MultiPendulumExample::keyboardCallback(int key, int state) { + + //b3Printf("Key pressed: %d in state %d \n",key,state); + + //key 1, key 2, key 3 + switch (key) { + case '1' /*ASCII for 1*/: { + + //assumption: Sphere are aligned in Z axis + btScalar newLimit = btScalar(gCurrentPendulumLength + 0.1); + + changePendulaLength(newLimit); + gCurrentPendulumLength = newLimit; + + b3Printf("Increase pendulum length to %f", gCurrentPendulumLength); + return true; + } + case '2' /*ASCII for 2*/: { + + //assumption: Sphere are aligned in Z axis + btScalar newLimit = btScalar(gCurrentPendulumLength - 0.1); + + //is being shortened beyond it's own length, we don't let the lower sphere to go over the upper one + if (0 <= newLimit) { + changePendulaLength(newLimit); + gCurrentPendulumLength = newLimit; + } + + b3Printf("Decrease pendulum length to %f", gCurrentPendulumLength); + return true; + } + case '3' /*ASCII for 3*/: { + applyPendulumForce(gDisplacementForce); + return true; + } + } + + return false; +} + +void MultiPendulumExample::applyPendulumForce(btScalar pendulumForce){ + if(pendulumForce != 0){ + b3Printf("Apply %f to pendulum",pendulumForce); + for (int i = 0; i < gDisplacedPendula; i++) { + if (gDisplacedPendula >= 0 && gDisplacedPendula <= gPendulaQty) + pendula[i]->applyCentralForce(btVector3(pendulumForce, 0, 0)); + } + } +} + +// GUI parameter modifiers + +void onMultiPendulaLengthChanged(float pendulaLength) { // Change the pendula length + if (mex){ + mex->changePendulaLength(pendulaLength); + } + //b3Printf("Pendula length changed to %f \n",sliderValue ); + +} + +void onMultiPendulaRestitutionChanged(float pendulaRestitution) { // change the pendula restitution + if (mex){ + mex->changePendulaRestitution(pendulaRestitution); + } + +} + +void floorMSliderValue(float notUsed) { // floor the slider values which should be integers + gPendulaQty = floor(gPendulaQty); + gDisplacedPendula = floor(gDisplacedPendula); +} + +void applyMForceWithForceScalar(float forceScalar) { + if(mex){ + btScalar appliedForce = forceScalar * gDisplacementForce; + + if(fabs(gForceScalar) < 0.2f) + gForceScalar = 0; + + mex->applyPendulumForce(appliedForce); + } +} + +CommonExampleInterface* ET_MultiPendulumCreateFunc( + CommonExampleOptions& options) { + mex = new MultiPendulumExample(options.m_guiHelper); + return mex; +} diff --git a/examples/ExtendedTutorials/MultiPendulum.h b/examples/ExtendedTutorials/MultiPendulum.h new file mode 100644 index 000000000..336c1be3c --- /dev/null +++ b/examples/ExtendedTutorials/MultiPendulum.h @@ -0,0 +1,22 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef ET_MULTI_PENDULUM_EXAMPLE_H +#define ET_MULTI_PENDULUM_EXAMPLE_H + +class CommonExampleInterface* ET_MultiPendulumCreateFunc(struct CommonExampleOptions& options); + + +#endif //ET_MULTI_PENDULUM_EXAMPLE_H diff --git a/examples/ExtendedTutorials/NewtonsCradle.cpp b/examples/ExtendedTutorials/NewtonsCradle.cpp new file mode 100644 index 000000000..0d4c52095 --- /dev/null +++ b/examples/ExtendedTutorials/NewtonsCradle.cpp @@ -0,0 +1,380 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2015 Google Inc. http://bulletphysics.org + + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include "NewtonsCradle.h" + +#include // TODO: Should I use another data structure? +#include + +#include "btBulletDynamicsCommon.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonParameterInterface.h" + +static btScalar gPendulaQty = 5; // Number of pendula in newton's cradle +//TODO: This would actually be an Integer, but the Slider does not like integers, so I floor it when changed + +static btScalar gDisplacedPendula = 1; // number of displaced pendula +//TODO: This is an int as well + +static btScalar gPendulaRestitution = 1; // pendula restitution when hitting against each other + +static btScalar gSphereRadius = 1; // pendula radius + +static btScalar gCurrentPendulumLength = 8; // current pendula length + +static btScalar gInitialPendulumLength = 8; // default pendula length + +static btScalar gDisplacementForce = 30; // default force to displace the pendula + +static btScalar gForceScalar = 0; // default force scalar to apply a displacement + +struct NewtonsCradleExample: public CommonRigidBodyBase { + NewtonsCradleExample(struct GUIHelperInterface* helper) : + CommonRigidBodyBase(helper) { + } + virtual ~NewtonsCradleExample() { + } + virtual void initPhysics(); + virtual void renderScene(); + virtual void createPendulum(btSphereShape* colShape, const btVector3& position, btScalar length, btScalar mass); + virtual void changePendulaLength(btScalar length); + virtual void changePendulaRestitution(btScalar restitution); + virtual void stepSimulation(float deltaTime); + virtual bool keyboardCallback(int key, int state); + virtual void applyPendulumForce(btScalar pendulumForce); + void resetCamera() { + float dist = 41; + float pitch = 52; + float yaw = 35; + float targetPos[3] = { 0, 0.46, 0 }; + m_guiHelper->resetCamera(dist, pitch, yaw, targetPos[0], targetPos[1], + targetPos[2]); + } + + std::vector constraints; // keep a handle to the slider constraints + std::vector pendula; // keep a handle to the pendula +}; + +static NewtonsCradleExample* nex = NULL; + +void onPendulaLengthChanged(float pendulaLength); // Change the pendula length + +void onPendulaRestitutionChanged(float pendulaRestitution); // change the pendula restitution + +void floorSliderValue(float notUsed); // floor the slider values which should be integers + +void applyForceWithForceScalar(float forceScalar); + +void NewtonsCradleExample::initPhysics() { + + { // create a slider to change the number of pendula + SliderParams slider("Number of Pendula", &gPendulaQty); + slider.m_minVal = 1; + slider.m_maxVal = 50; + slider.m_callback = floorSliderValue; // hack to get integer values + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to change the number of displaced pendula + SliderParams slider("Number of Displaced Pendula", &gDisplacedPendula); + slider.m_minVal = 0; + slider.m_maxVal = 49; + slider.m_callback = floorSliderValue; // hack to get integer values + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to change the pendula restitution + SliderParams slider("Pendula Restitution", &gPendulaRestitution); + slider.m_minVal = 0; + slider.m_maxVal = 1; + slider.m_clampToNotches = false; + slider.m_callback = onPendulaRestitutionChanged; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to change the pendulum length + SliderParams slider("Pendula Length", &gCurrentPendulumLength); + slider.m_minVal = 0; + slider.m_maxVal = 49; + slider.m_clampToNotches = false; + slider.m_callback = onPendulaLengthChanged; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to change the force to displace the lowest pendulum + SliderParams slider("Displacement force", &gDisplacementForce); + slider.m_minVal = 0.1; + slider.m_maxVal = 200; + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to apply the force by slider + SliderParams slider("Apply displacement force", &gForceScalar); + slider.m_minVal = -1; + slider.m_maxVal = 1; + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + m_guiHelper->setUpAxis(1); + + createEmptyDynamicsWorld(); + + // create a debug drawer + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + if (m_dynamicsWorld->getDebugDrawer()) + m_dynamicsWorld->getDebugDrawer()->setDebugMode( + btIDebugDraw::DBG_DrawWireframe + + btIDebugDraw::DBG_DrawContactPoints + + btIDebugDraw::DBG_DrawConstraints + + btIDebugDraw::DBG_DrawConstraintLimits); + + { // create the pendula starting at the indicated position below and where each pendulum has the following mass + btScalar pendulumMass(1.f); + + btVector3 position(0.0f,15.0f,0.0f); // initial left-most pendulum position + btQuaternion orientation(0,0,0,1); // orientation of the pendula + + // Re-using the same collision is better for memory usage and performance + btSphereShape* pendulumShape = new btSphereShape(gSphereRadius); + m_collisionShapes.push_back(pendulumShape); + + for (int i = 0; i < floor(gPendulaQty); i++) { + + // create pendulum + createPendulum(pendulumShape, position, gInitialPendulumLength, pendulumMass); + + // displace the pendula 1.05 sphere size, so that they all nearly touch (small spacings in between + position.setX(position.x()-2.1f * gSphereRadius); + } + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void NewtonsCradleExample::stepSimulation(float deltaTime) { + + applyForceWithForceScalar(gForceScalar); // apply force defined by apply force slider + + if (m_dynamicsWorld) { + m_dynamicsWorld->stepSimulation(deltaTime); + } +} + +void NewtonsCradleExample::createPendulum(btSphereShape* colShape, const btVector3& position, btScalar length, btScalar mass) { + + // The pendulum looks like this (names when built): + // O topSphere + // | + // O bottomSphere + + //create a dynamic pendulum + btTransform startTransform; + startTransform.setIdentity(); + + // position the top sphere above ground with a moving x position + startTransform.setOrigin(position); + startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation + btRigidBody* topSphere = createRigidBody(mass, startTransform, colShape); + + // position the bottom sphere below the top sphere + startTransform.setOrigin( + btVector3(position.x(), btScalar(position.y() - length), + position.z())); + + startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation + btRigidBody* bottomSphere = createRigidBody(mass, startTransform, colShape); + bottomSphere->setFriction(0); // we do not need friction here + pendula.push_back(bottomSphere); + + // disable the deactivation when objects do not move anymore + topSphere->setActivationState(DISABLE_DEACTIVATION); + bottomSphere->setActivationState(DISABLE_DEACTIVATION); + + bottomSphere->setRestitution(gPendulaRestitution); // set pendula restitution + + //make the top sphere position "fixed" to the world by attaching with a point to point constraint + // The pivot is defined in the reference frame of topSphere, so the attachment is exactly at the center of the topSphere + btVector3 constraintPivot(btVector3(0.0f, 0.0f, 0.0f)); + btPoint2PointConstraint* p2pconst = new btPoint2PointConstraint(*topSphere, + constraintPivot); + + p2pconst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing + + // add the constraint to the world + m_dynamicsWorld->addConstraint(p2pconst, true); + + //create constraint between spheres + // this is represented by the constraint pivot in the local frames of reference of both constrained spheres + // furthermore we need to rotate the constraint appropriately to orient it correctly in space + btTransform constraintPivotInTopSphereRF, constraintPivotInBottomSphereRF; + + constraintPivotInTopSphereRF.setIdentity(); + constraintPivotInBottomSphereRF.setIdentity(); + + // the slider constraint is x aligned per default, but we want it to be y aligned, therefore we rotate it + btQuaternion qt; + qt.setEuler(0, 0, -SIMD_HALF_PI); + constraintPivotInTopSphereRF.setRotation(qt); //we use Y like up Axis + constraintPivotInBottomSphereRF.setRotation(qt); //we use Y like up Axis + + //Obtain the position of topSphere in local reference frame of bottomSphere (the pivot is therefore in the center of topSphere) + btVector3 topSphereInBottomSphereRF = + (bottomSphere->getWorldTransform().inverse()( + topSphere->getWorldTransform().getOrigin())); + constraintPivotInBottomSphereRF.setOrigin(topSphereInBottomSphereRF); + + btSliderConstraint* sliderConst = new btSliderConstraint(*topSphere, + *bottomSphere, constraintPivotInTopSphereRF, constraintPivotInBottomSphereRF, true); + + sliderConst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing + + // set limits + // the initial setup of the constraint defines the origins of the limit dimensions, + // therefore we set both limits directly to the current position of the topSphere + sliderConst->setLowerLinLimit(btScalar(0)); + sliderConst->setUpperLinLimit(btScalar(0)); + sliderConst->setLowerAngLimit(btScalar(0)); + sliderConst->setUpperAngLimit(btScalar(0)); + constraints.push_back(sliderConst); + + // add the constraint to the world + m_dynamicsWorld->addConstraint(sliderConst, true); +} + +void NewtonsCradleExample::changePendulaLength(btScalar length) { + btScalar lowerLimit = -gInitialPendulumLength; + for (std::vector::iterator sit = constraints.begin(); + sit != constraints.end(); sit++) { + btAssert((*sit) && "Null constraint"); + + //if the pendulum is being shortened beyond it's own length, we don't let the lower sphere to go past the upper one + if (lowerLimit <= length) { + (*sit)->setLowerLinLimit(length + lowerLimit); + (*sit)->setUpperLinLimit(length + lowerLimit); + } + } +} + +void NewtonsCradleExample::changePendulaRestitution(btScalar restitution) { + for (std::vector::iterator rit = pendula.begin(); + rit != pendula.end(); rit++) { + btAssert((*rit) && "Null constraint"); + + (*rit)->setRestitution(restitution); + } +} + +void NewtonsCradleExample::renderScene() { + CommonRigidBodyBase::renderScene(); +} + +bool NewtonsCradleExample::keyboardCallback(int key, int state) { + //b3Printf("Key pressed: %d in state %d \n",key,state); + + //key 1, key 2, key 3 + switch (key) { + case '1' /*ASCII for 1*/: { + + //assumption: Sphere are aligned in Z axis + btScalar newLimit = btScalar(gCurrentPendulumLength + 0.1); + + changePendulaLength(newLimit); + gCurrentPendulumLength = newLimit; + + b3Printf("Increase pendulum length to %f", gCurrentPendulumLength); + return true; + } + case '2' /*ASCII for 2*/: { + + //assumption: Sphere are aligned in Z axis + btScalar newLimit = btScalar(gCurrentPendulumLength - 0.1); + + //is being shortened beyond it's own length, we don't let the lower sphere to go over the upper one + if (0 <= newLimit) { + changePendulaLength(newLimit); + gCurrentPendulumLength = newLimit; + } + + b3Printf("Decrease pendulum length to %f", gCurrentPendulumLength); + return true; + } + case '3' /*ASCII for 3*/: { + applyPendulumForce(gDisplacementForce); + return true; + } + } + + return false; +} + +void NewtonsCradleExample::applyPendulumForce(btScalar pendulumForce){ + if(pendulumForce != 0){ + b3Printf("Apply %f to pendulum",pendulumForce); + for (int i = 0; i < gDisplacedPendula; i++) { + if (gDisplacedPendula >= 0 && gDisplacedPendula <= gPendulaQty) + pendula[i]->applyCentralForce(btVector3(pendulumForce, 0, 0)); + } + } +} + +// GUI parameter modifiers + +void onPendulaLengthChanged(float pendulaLength) { + if (nex){ + nex->changePendulaLength(pendulaLength); + //b3Printf("Pendula length changed to %f \n",sliderValue ); + } +} + +void onPendulaRestitutionChanged(float pendulaRestitution) { + if (nex){ + nex->changePendulaRestitution(pendulaRestitution); + } +} + +void floorSliderValue(float notUsed) { + gPendulaQty = floor(gPendulaQty); + gDisplacedPendula = floor(gDisplacedPendula); + +} + +void applyForceWithForceScalar(float forceScalar) { + if(nex){ + btScalar appliedForce = forceScalar * gDisplacementForce; + + if(fabs(gForceScalar) < 0.2f) + gForceScalar = 0; + + nex->applyPendulumForce(appliedForce); + } +} + +CommonExampleInterface* ET_NewtonsCradleCreateFunc( + CommonExampleOptions& options) { + nex = new NewtonsCradleExample(options.m_guiHelper); + return nex; +} diff --git a/examples/ExtendedTutorials/NewtonsCradle.h b/examples/ExtendedTutorials/NewtonsCradle.h new file mode 100644 index 000000000..028754f7e --- /dev/null +++ b/examples/ExtendedTutorials/NewtonsCradle.h @@ -0,0 +1,22 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef ET_NEWTONS_CRADLE_EXAMPLE_H +#define ET_NEWTONS_CRADLE_EXAMPLE_H + +class CommonExampleInterface* ET_NewtonsCradleCreateFunc(struct CommonExampleOptions& options); + + +#endif //ET_NEWTONS_CRADLE_EXAMPLE_H diff --git a/examples/ExtendedTutorials/NewtonsRopeCradle.cpp b/examples/ExtendedTutorials/NewtonsRopeCradle.cpp new file mode 100644 index 000000000..94c96a71f --- /dev/null +++ b/examples/ExtendedTutorials/NewtonsRopeCradle.cpp @@ -0,0 +1,387 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2015 Google Inc. http://bulletphysics.org + + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include "NewtonsRopeCradle.h" + +#include // TODO: Should I use another data structure? +#include + +#include "btBulletDynamicsCommon.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "../CommonInterfaces/CommonRigidBodyBase.h" + +#include "BulletSoftBody/btSoftRigidDynamicsWorld.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "../CommonInterfaces/CommonParameterInterface.h" + +static btScalar gPendulaQty = 5; // Number of pendula in newton's cradle +//TODO: This would actually be an Integer, but the Slider does not like integers, so I floor it when changed + +static btScalar gDisplacedPendula = 1; // number of displaced pendula +//TODO: This is an int as well + +static btScalar gPendulaRestitution = 1; // pendula restition when hitting against each other + +static btScalar gSphereRadius = 1; // pendula radius + +static btScalar gInitialPendulumWidth = 4; // default pendula width + +static btScalar gInitialPendulumHeight = 8; // default pendula height + +static btScalar gRopeResolution = 1; // default rope resolution (number of links as in a chain) + +static btScalar gDisplacementForce = 30; // default force to displace the pendula + +static btScalar gForceScalar = 0; // default force scalar to apply a displacement + +struct NewtonsRopeCradleExample : public CommonRigidBodyBase { + NewtonsRopeCradleExample(struct GUIHelperInterface* helper) : + CommonRigidBodyBase(helper) { + } + virtual ~NewtonsRopeCradleExample(){} + virtual void initPhysics(); + virtual void stepSimulation(float deltaTime); + virtual void renderScene(); + virtual void applyPendulumForce(btScalar pendulumForce); + void createEmptyDynamicsWorld() + { + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + + m_solver = new btSequentialImpulseConstraintSolver; + + m_dynamicsWorld = new btSoftRigidDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); + m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); + + softBodyWorldInfo.m_broadphase = m_broadphase; + softBodyWorldInfo.m_dispatcher = m_dispatcher; + softBodyWorldInfo.m_gravity = m_dynamicsWorld->getGravity(); + softBodyWorldInfo.m_sparsesdf.Initialize(); + } + + virtual void createRopePendulum(btSphereShape* colShape, + const btVector3& position, const btQuaternion& pendulumOrientation, btScalar width, btScalar height, btScalar mass); + virtual void changePendulaRestitution(btScalar restitution); + virtual void connectWithRope(btRigidBody* body1, btRigidBody* body2); + virtual bool keyboardCallback(int key, int state); + + virtual btSoftRigidDynamicsWorld* getSoftDynamicsWorld() + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btSoftRigidDynamicsWorld*) m_dynamicsWorld; + } + void resetCamera() + { + float dist = 41; + float pitch = 52; + float yaw = 35; + float targetPos[3]={0,0.46,0}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } + + std::vector constraints; + std::vector pendula; + + btSoftBodyWorldInfo softBodyWorldInfo; + +}; + +static NewtonsRopeCradleExample* nex = NULL; + +void onRopePendulaRestitutionChanged(float pendulaRestitution); + +void floorRSliderValue(float notUsed); + +void applyRForceWithForceScalar(float forceScalar); + +void NewtonsRopeCradleExample::initPhysics() +{ + + { // create a slider to change the number of pendula + SliderParams slider("Number of Pendula", &gPendulaQty); + slider.m_minVal = 1; + slider.m_maxVal = 50; + slider.m_callback = floorRSliderValue; // hack to get integer values + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to change the number of displaced pendula + SliderParams slider("Number of Displaced Pendula", &gDisplacedPendula); + slider.m_minVal = 0; + slider.m_maxVal = 49; + slider.m_callback = floorRSliderValue; // hack to get integer values + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to change the pendula restitution + SliderParams slider("Pendula Restitution", &gPendulaRestitution); + slider.m_minVal = 0; + slider.m_maxVal = 1; + slider.m_clampToNotches = false; + slider.m_callback = onRopePendulaRestitutionChanged; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to change the rope resolution + SliderParams slider("Rope Resolution", &gRopeResolution); + slider.m_minVal = 1; + slider.m_maxVal = 20; + slider.m_clampToNotches = false; + slider.m_callback = floorRSliderValue; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to change the pendulum width + SliderParams slider("Pendulum Width", &gInitialPendulumWidth); + slider.m_minVal = 0; + slider.m_maxVal = 40; + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to change the pendulum height + SliderParams slider("Pendulum Height", &gInitialPendulumHeight); + slider.m_minVal = 0; + slider.m_maxVal = 40; + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to change the force to displace the lowest pendulum + SliderParams slider("Displacement force", &gDisplacementForce); + slider.m_minVal = 0.1; + slider.m_maxVal = 200; + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to apply the force by slider + SliderParams slider("Apply displacement force", &gForceScalar); + slider.m_minVal = -1; + slider.m_maxVal = 1; + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + m_guiHelper->setUpAxis(1); + + createEmptyDynamicsWorld(); + + // create a debug drawer + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + if (m_dynamicsWorld->getDebugDrawer()) + m_dynamicsWorld->getDebugDrawer()->setDebugMode( + btIDebugDraw::DBG_DrawWireframe + + btIDebugDraw::DBG_DrawContactPoints + + btIDebugDraw::DBG_DrawConstraints + + btIDebugDraw::DBG_DrawConstraintLimits); + + { // create the pendula starting at the indicated position below and where each pendulum has the following mass + btScalar pendulumMass(1.0f); + + btVector3 position(0.0f,15.0f,0.0f); // initial left-most pendulum position + btQuaternion orientation(0,0,0,1); // orientation of the pendula + + // Re-using the same collision is better for memory usage and performance + btSphereShape* pendulumShape = new btSphereShape(gSphereRadius); + m_collisionShapes.push_back(pendulumShape); + + for (int i = 0; i < floor(gPendulaQty); i++) { + + // create pendulum + createRopePendulum(pendulumShape, position, orientation,gInitialPendulumWidth, + gInitialPendulumHeight, pendulumMass); + + // displace the pendula 1.05 sphere size, so that they all nearly touch (small spacings in between) + position.setX(position.x()-2.1f * gSphereRadius); + } + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void NewtonsRopeCradleExample::connectWithRope(btRigidBody* body1, btRigidBody* body2) +{ + btSoftBody* softBodyRope0 = btSoftBodyHelpers::CreateRope(softBodyWorldInfo,body1->getWorldTransform().getOrigin(),body2->getWorldTransform().getOrigin(),gRopeResolution,0); + softBodyRope0->setTotalMass(0.1f); + + softBodyRope0->appendAnchor(0,body1); + softBodyRope0->appendAnchor(softBodyRope0->m_nodes.size()-1,body2); + + softBodyRope0->m_cfg.piterations = 5; + softBodyRope0->m_cfg.kDP = 0.005f; + softBodyRope0->m_cfg.kSHR = 1; + softBodyRope0->m_cfg.kCHR = 1; + softBodyRope0->m_cfg.kKHR = 1; + + getSoftDynamicsWorld()->addSoftBody(softBodyRope0); +} + +void NewtonsRopeCradleExample::stepSimulation(float deltaTime) { + + applyRForceWithForceScalar(gForceScalar); // apply force defined by apply force slider + + if (m_dynamicsWorld) { + m_dynamicsWorld->stepSimulation(deltaTime); + } +} + +void NewtonsRopeCradleExample::createRopePendulum(btSphereShape* colShape, + const btVector3& position, const btQuaternion& pendulumOrientation, btScalar width, btScalar height, btScalar mass) { + + // The pendulum looks like this (names when built): + // O O topSphere1 topSphere2 + // \ / + // O bottomSphere + + //create a dynamic pendulum + btTransform startTransform; + startTransform.setIdentity(); + + // calculate sphere positions + btVector3 topSphere1RelPosition(0,0,width); + btVector3 topSphere2RelPosition(0,0,-width); + btVector3 bottomSphereRelPosition(0,-height,0); + + + // position the top sphere above ground with appropriate orientation + startTransform.setOrigin(btVector3(0,0,0)); // no translation intitially + startTransform.setRotation(pendulumOrientation); // pendulum rotation + startTransform.setOrigin(startTransform * topSphere1RelPosition); // rotate this position + startTransform.setOrigin(position + startTransform.getOrigin()); // add non-rotated position to the relative position + btRigidBody* topSphere1 = createRigidBody(0, startTransform, colShape); // make top sphere static + + // position the top sphere above ground with appropriate orientation + startTransform.setOrigin(btVector3(0,0,0)); // no translation intitially + startTransform.setRotation(pendulumOrientation); // pendulum rotation + startTransform.setOrigin(startTransform * topSphere2RelPosition); // rotate this position + startTransform.setOrigin(position + startTransform.getOrigin()); // add non-rotated position to the relative position + btRigidBody* topSphere2 = createRigidBody(0, startTransform, colShape); // make top sphere static + + // position the bottom sphere below the top sphere + startTransform.setOrigin(btVector3(0,0,0)); // no translation intitially + startTransform.setRotation(pendulumOrientation); // pendulum rotation + startTransform.setOrigin(startTransform * bottomSphereRelPosition); // rotate this position + startTransform.setOrigin(position + startTransform.getOrigin()); // add non-rotated position to the relative position + btRigidBody* bottomSphere = createRigidBody(mass, startTransform, colShape); + bottomSphere->setFriction(0); // we do not need friction here + pendula.push_back(bottomSphere); + + // disable the deactivation when objects do not move anymore + topSphere1->setActivationState(DISABLE_DEACTIVATION); + topSphere2->setActivationState(DISABLE_DEACTIVATION); + bottomSphere->setActivationState(DISABLE_DEACTIVATION); + + bottomSphere->setRestitution(gPendulaRestitution); // set pendula restitution + + // add ropes between spheres + connectWithRope(topSphere1, bottomSphere); + connectWithRope(topSphere2, bottomSphere); +} + +void NewtonsRopeCradleExample::renderScene() +{ + CommonRigidBodyBase::renderScene(); + btSoftRigidDynamicsWorld* softWorld = getSoftDynamicsWorld(); + + for ( int i=0;igetSoftBodyArray().size();i++) + { + btSoftBody* psb=(btSoftBody*)softWorld->getSoftBodyArray()[i]; + //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + btSoftBodyHelpers::DrawFrame(psb,softWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb,softWorld->getDebugDrawer(),softWorld->getDrawFlags()); + } + } +} + +void NewtonsRopeCradleExample::changePendulaRestitution(btScalar restitution) { + for (std::vector::iterator rit = pendula.begin(); + rit != pendula.end(); rit++) { + btAssert((*rit) && "Null constraint"); + + (*rit)->setRestitution(restitution); + } +} + +bool NewtonsRopeCradleExample::keyboardCallback(int key, int state) { + //b3Printf("Key pressed: %d in state %d \n",key,state); + + // key 3 + switch (key) { + case '3' /*ASCII for 3*/: { + applyPendulumForce(gDisplacementForce); + return true; + } + } + + return false; +} + +void NewtonsRopeCradleExample::applyPendulumForce(btScalar pendulumForce){ + if(pendulumForce != 0){ + b3Printf("Apply %f to pendulum",pendulumForce); + for (int i = 0; i < gDisplacedPendula; i++) { + if (gDisplacedPendula >= 0 && gDisplacedPendula <= gPendulaQty) + pendula[i]->applyCentralForce(btVector3(pendulumForce, 0, 0)); + } + } +} + +// GUI parameter modifiers + +void onRopePendulaRestitutionChanged(float pendulaRestitution) { + if (nex){ + nex->changePendulaRestitution(pendulaRestitution); + } +} + +void floorRSliderValue(float notUsed) { + gPendulaQty = floor(gPendulaQty); + gDisplacedPendula = floor(gDisplacedPendula); + gRopeResolution = floor(gRopeResolution); +} + +void applyRForceWithForceScalar(float forceScalar) { + if(nex){ + btScalar appliedForce = forceScalar * gDisplacementForce; + + if(fabs(gForceScalar) < 0.2f) + gForceScalar = 0; + + nex->applyPendulumForce(appliedForce); + } +} + +CommonExampleInterface* ET_NewtonsRopeCradleCreateFunc( + CommonExampleOptions& options) { + nex = new NewtonsRopeCradleExample(options.m_guiHelper); + return nex; +} diff --git a/examples/ExtendedTutorials/NewtonsRopeCradle.h b/examples/ExtendedTutorials/NewtonsRopeCradle.h new file mode 100644 index 000000000..3edbcd5af --- /dev/null +++ b/examples/ExtendedTutorials/NewtonsRopeCradle.h @@ -0,0 +1,22 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H +#define ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H + +class CommonExampleInterface* ET_NewtonsRopeCradleCreateFunc(struct CommonExampleOptions& options); + + +#endif //ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H diff --git a/examples/ExtendedTutorials/RigidBodyFromObj.cpp b/examples/ExtendedTutorials/RigidBodyFromObj.cpp index 64d52793d..9e09f9dd9 100644 --- a/examples/ExtendedTutorials/RigidBodyFromObj.cpp +++ b/examples/ExtendedTutorials/RigidBodyFromObj.cpp @@ -134,7 +134,8 @@ void RigidBodyFromObjExample::initPhysics() int shapeId = m_guiHelper->registerGraphicsShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, &glmesh->m_indices->at(0), - glmesh->m_numIndices); + glmesh->m_numIndices, + B3_GL_TRIANGLES, -1); shape->setUserIndex(shapeId); int renderInstance = m_guiHelper->registerGraphicsInstance(shapeId,pos,orn,color,scaling); body->setUserIndex(renderInstance); diff --git a/examples/ExtendedTutorials/premake4.lua b/examples/ExtendedTutorials/premake4.lua index 92ac12d45..41126e0af 100644 --- a/examples/ExtendedTutorials/premake4.lua +++ b/examples/ExtendedTutorials/premake4.lua @@ -39,7 +39,7 @@ files { "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", - "../ThirdPartyLibs/stb_image/stb_image.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } @@ -91,7 +91,9 @@ files { "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", - "../ThirdPartyLibs/stb_image/stb_image.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", + "../Utils/b3Clock.cpp", + "../Utils/b3Clock.h", } if os.is("Linux") then initX11() end @@ -156,7 +158,7 @@ files { "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", - "../ThirdPartyLibs/stb_image/stb_image.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } if os.is("Linux") then initX11() end diff --git a/examples/Importers/ImportBsp/ImportBspExample.cpp b/examples/Importers/ImportBsp/ImportBspExample.cpp index 038342379..eec8a4fb9 100644 --- a/examples/Importers/ImportBsp/ImportBspExample.cpp +++ b/examples/Importers/ImportBsp/ImportBspExample.cpp @@ -285,7 +285,7 @@ char* makeExeToBspFilename(const char* lpCmdLine) } -struct CommonExampleInterface* ImportBspCreateFunc(struct CommonExampleOptions& options) +CommonExampleInterface* ImportBspCreateFunc(struct CommonExampleOptions& options) { BspDemo* demo = new BspDemo(options.m_guiHelper); diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 9b6eb8ae1..1c24c2eba 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -108,7 +108,8 @@ struct BulletErrorLogger : public ErrorLogger bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) { - + if (strlen(fileName)==0) + return false; //int argc=0; char relativeFileName[1024]; @@ -132,7 +133,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) std::fstream xml_file(relativeFileName, std::fstream::in); - while ( xml_file.good() ) + while ( xml_file.good()) { std::string line; std::getline( xml_file, line); @@ -969,16 +970,17 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP // graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); //graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); - CommonRenderInterface* renderer = m_data->m_guiHelper->getRenderInterface(); + //CommonRenderInterface* renderer = m_data->m_guiHelper->getRenderInterface(); - if (renderer) + if (1) { int textureIndex = -1; if (textures.size()) { - textureIndex = renderer->registerTexture(textures[0].textureData,textures[0].m_width,textures[0].m_height); + + textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData,textures[0].m_width,textures[0].m_height); } - graphicsIndex = renderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(),B3_GL_TRIANGLES,textureIndex); + graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(),B3_GL_TRIANGLES,textureIndex); } } @@ -1003,6 +1005,17 @@ bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const return false; } +bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const +{ + UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); + if (linkPtr) + { + const UrdfLink* link = *linkPtr; + contactInfo = link->m_contactInfo; + return true; + } + return false; +} void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj) const { diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 14032e388..ea945aa33 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -37,6 +37,8 @@ public: virtual std::string getLinkName(int linkIndex) const; virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const; + + virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const; virtual std::string getJointName(int linkIndex) const; diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index 92a917fd7..4e9ddf6c4 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -65,7 +65,8 @@ btAlignedObjectArray gFileNameArray; struct ImportUrdfInternalData { ImportUrdfInternalData() - :m_numMotors(0) + :m_numMotors(0), + m_mb(0) { for (int i=0;isetGravity(gravity); @@ -223,7 +227,6 @@ void ImportUrdfSetup::initPhysics() { - btMultiBody* mb = 0; //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user @@ -232,8 +235,10 @@ void ImportUrdfSetup::initPhysics() MyMultiBodyCreator creation(m_guiHelper); ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix()); - mb = creation.getBulletMultiBody(); - + m_data->m_rb = creation.getRigidBody(); + m_data->m_mb = creation.getBulletMultiBody(); + btMultiBody* mb = m_data->m_mb; + if (m_useMultiBody && mb ) { std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); @@ -337,7 +342,7 @@ void ImportUrdfSetup::initPhysics() - bool createGround=true; + bool createGround=false; if (createGround) { btVector3 groundHalfExtents(20,20,20); @@ -357,8 +362,7 @@ void ImportUrdfSetup::initPhysics() m_guiHelper->createRigidBodyGraphicsObject(body,color); } - ///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates. - m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.); + } #ifdef TEST_MULTIBODY_SERIALIZATION diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp index 28e9023d8..3d34e53f2 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp @@ -13,6 +13,7 @@ MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper) :m_bulletMultiBody(0), + m_rigidBody(0), m_guiHelper(guiHelper) { } @@ -31,10 +32,10 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc { btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal); rbci.m_startWorldTransform = initialWorldTrans; - btRigidBody* body = new btRigidBody(rbci); - body->forceActivationState(DISABLE_DEACTIVATION); + m_rigidBody = new btRigidBody(rbci); + m_rigidBody->forceActivationState(DISABLE_DEACTIVATION); - return body; + return m_rigidBody; } class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody) diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h index 7334fbd90..453f97311 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h @@ -24,6 +24,7 @@ class MyMultiBodyCreator : public MultiBodyCreationInterface protected: btMultiBody* m_bulletMultiBody; + btRigidBody* m_rigidBody; struct GUIHelperInterface* m_guiHelper; @@ -62,6 +63,10 @@ public: virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex); btMultiBody* getBulletMultiBody(); + btRigidBody* getRigidBody() + { + return m_rigidBody; + } int getNum6DofConstraints() const { diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index fbe9f13f8..a64bf44f5 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -392,9 +392,18 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col); - btScalar friction = 0.5f; + URDFLinkContactInfo contactInfo; + u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); - col->setFriction(friction); + if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION)!=0) + { + col->setFriction(contactInfo.m_lateralFriction); + } + if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION)!=0) + { + col->setRollingFriction(contactInfo.m_rollingFriction); + } + if (mbLinkIndex>=0) //???? double-check +/- 1 { diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 345ef7d5f..e3299eda5 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -6,6 +6,7 @@ #include "LinearMath/btTransform.h" #include "URDFJointTypes.h" + class URDFImporterInterface { @@ -28,6 +29,9 @@ public: virtual std::string getLinkName(int linkIndex) const =0; /// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;} + + ///this API will likely change, don't override it! + virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const { return false;} virtual std::string getJointName(int linkIndex) const = 0; diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h index 0c50d2655..65d615ee3 100644 --- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -11,6 +11,33 @@ enum UrdfJointTypes URDFPlanarJoint, URDFFixedJoint, }; +#include "LinearMath/btScalar.h" + +enum URDF_LinkContactFlags +{ + URDF_CONTACT_HAS_LATERAL_FRICTION=1, + URDF_CONTACT_HAS_ROLLING_FRICTION=2, + URDF_CONTACT_HAS_CONTACT_CFM=4, + URDF_CONTACT_HAS_CONTACT_ERP=8 +}; + +struct URDFLinkContactInfo +{ + btScalar m_lateralFriction; + btScalar m_rollingFriction; + btScalar m_contactCfm; + btScalar m_contactErp; + int m_flags; + + URDFLinkContactInfo() + :m_lateralFriction(0.5), + m_rollingFriction(0), + m_contactCfm(0), + m_contactErp(0) + { + m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION; + } +}; #endif //URDF_JOINT_TYPES_H diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index a6ec0a5c4..002a3cf93 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -569,6 +569,31 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi } } + { + //optional 'contact' parameters + TiXmlElement* ci = config->FirstChildElement("contact"); + if (ci) + { + TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction"); + if (friction_xml) + { + if (m_parseSDF) + { + link.m_contactInfo.m_lateralFriction = urdfLexicalCast(friction_xml->GetText()); + } else + { + if (!friction_xml->Attribute("value")) + { + logger->reportError("Link/contact: lateral_friction element must have value attribute"); + return false; + } + + link.m_contactInfo.m_lateralFriction = urdfLexicalCast(friction_xml->Attribute("value")); + } + } + } + } + // Inertial (optional) TiXmlElement *i = config->FirstChildElement("inertial"); if (i) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index b83c0b3c8..8e6dc6fb7 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -99,6 +99,8 @@ struct UrdfLink int m_linkIndex; + URDFLinkContactInfo m_contactInfo; + UrdfLink() :m_parentLink(0), m_parentJoint(0) diff --git a/examples/InverseDynamics/premake4.lua b/examples/InverseDynamics/premake4.lua index 53c4fa2ce..75105e702 100644 --- a/examples/InverseDynamics/premake4.lua +++ b/examples/InverseDynamics/premake4.lua @@ -91,7 +91,8 @@ files { "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", - "../ThirdPartyLibs/stb_image/stb_image.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", + "../Utils/b3Clock.cpp", } if os.is("Linux") then initX11() end diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp index 8647b8734..9ff47b8a0 100644 --- a/examples/MultiBody/TestJointTorqueSetup.cpp +++ b/examples/MultiBody/TestJointTorqueSetup.cpp @@ -367,9 +367,8 @@ void TestJointTorqueSetup::initPhysics() btSerializer* s = new btDefaultSerializer; m_dynamicsWorld->serialize(s); - b3ResourcePath p; char resourcePath[1024]; - if (p.findResourcePath("multibody.bullet",resourcePath,1024)) + if (b3ResourcePath::findResourcePath("multibody.bullet",resourcePath,1024)) { FILE* f = fopen(resourcePath,"wb"); fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f); diff --git a/examples/MultiThreading/MultiThreadingExample.cpp b/examples/MultiThreading/MultiThreadingExample.cpp index 2e9f9ee15..1969912b9 100644 --- a/examples/MultiThreading/MultiThreadingExample.cpp +++ b/examples/MultiThreading/MultiThreadingExample.cpp @@ -119,7 +119,7 @@ struct SampleThreadLocalStorage void SampleThreadFunc(void* userPtr,void* lsMemory) { - printf("thread started\n"); + printf("SampleThreadFunc thread started\n"); SampleThreadLocalStorage* localStorage = (SampleThreadLocalStorage*) lsMemory; diff --git a/examples/MultiThreading/b3PosixThreadSupport.cpp b/examples/MultiThreading/b3PosixThreadSupport.cpp index 74cdfaba2..c19ab1b48 100644 --- a/examples/MultiThreading/b3PosixThreadSupport.cpp +++ b/examples/MultiThreading/b3PosixThreadSupport.cpp @@ -53,7 +53,7 @@ static sem_t* createSem(const char* baseName) #ifdef NAMED_SEMAPHORES /// Named semaphore begin char name[32]; - snprintf(name, 32, "/%s-%d-%4.4d", baseName, getpid(), semCount++); + snprintf(name, 32, "/%8.s-%4.d-%4.4d", baseName, getpid(), semCount++); sem_t* tempSem = sem_open(name, O_CREAT, 0600, 0); if (tempSem != reinterpret_cast(SEM_FAILED)) diff --git a/examples/MultiThreading/b3Win32ThreadSupport.cpp b/examples/MultiThreading/b3Win32ThreadSupport.cpp index 508b89348..199533b1b 100644 --- a/examples/MultiThreading/b3Win32ThreadSupport.cpp +++ b/examples/MultiThreading/b3Win32ThreadSupport.cpp @@ -223,7 +223,8 @@ bool b3Win32ThreadSupport::isTaskCompleted(int *puiArgument0, int *puiArgument1, void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threadConstructionInfo) { - + static int uniqueId = 0; + uniqueId++; m_activeThreadStatus.resize(threadConstructionInfo.m_numThreads); m_completeHandles.resize(threadConstructionInfo.m_numThreads); @@ -244,18 +245,40 @@ void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threa threadStatus.m_userPtr=0; - sprintf(threadStatus.m_eventStartHandleName,"eventStart%s%d",threadConstructionInfo.m_uniqueName,i); + sprintf(threadStatus.m_eventStartHandleName,"es%.8s%d%d",threadConstructionInfo.m_uniqueName,uniqueId,i); threadStatus.m_eventStartHandle = CreateEventA (0,false,false,threadStatus.m_eventStartHandleName); - sprintf(threadStatus.m_eventCompletetHandleName,"eventComplete%s%d",threadConstructionInfo.m_uniqueName,i); + sprintf(threadStatus.m_eventCompletetHandleName,"ec%.8s%d%d",threadConstructionInfo.m_uniqueName,uniqueId,i); threadStatus.m_eventCompletetHandle = CreateEventA (0,false,false,threadStatus.m_eventCompletetHandleName); m_completeHandles[i] = threadStatus.m_eventCompletetHandle; HANDLE handle = CreateThread(lpThreadAttributes,dwStackSize,lpStartAddress,lpParameter, dwCreationFlags,lpThreadId); - //SetThreadPriority(handle,THREAD_PRIORITY_HIGHEST); - SetThreadPriority(handle,THREAD_PRIORITY_TIME_CRITICAL); + switch(threadConstructionInfo.m_priority) + { + case 0: + { + SetThreadPriority(handle,THREAD_PRIORITY_HIGHEST); + break; + } + case 1: + { + SetThreadPriority(handle,THREAD_PRIORITY_TIME_CRITICAL); + break; + } + case 2: + { + SetThreadPriority(handle,THREAD_PRIORITY_BELOW_NORMAL); + break; + } + + default: + { + + } + } + SetThreadAffinityMask(handle, 1<m_totalNumInstances = 0; + for (int i=0;im_index_vbo) @@ -276,6 +278,7 @@ void GLInstancingRenderer::removeAllInstances() m_graphicsInstances.clear(); } + GLInstancingRenderer::~GLInstancingRenderer() { delete m_data->m_shadowMap; @@ -323,6 +326,7 @@ void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* positi */ } + void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int srcIndex) { m_data->m_instance_colors_ptr[srcIndex*4+0]=float(color[0]); @@ -340,7 +344,19 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(float* color, int srcIn m_data->m_instance_colors_ptr[srcIndex*4+3]=color[3]; } +void GLInstancingRenderer::writeSingleInstanceScaleToCPU(float* scale, int srcIndex) +{ + m_data->m_instance_scale_ptr[srcIndex*3+0]=scale[0]; + m_data->m_instance_scale_ptr[srcIndex*3+1]=scale[1]; + m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2]; +} +void GLInstancingRenderer::writeSingleInstanceScaleToCPU(double* scale, int srcIndex) +{ + m_data->m_instance_scale_ptr[srcIndex*3+0]=scale[0]; + m_data->m_instance_scale_ptr[srcIndex*3+1]=scale[1]; + m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2]; +} void GLInstancingRenderer::writeSingleInstanceTransformToGPU(float* position, float* orientation, int objectIndex) { @@ -386,25 +402,46 @@ void GLInstancingRenderer::writeTransforms() glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo); - glFlush(); + //glFlush(); b3Assert(glGetError() ==GL_NO_ERROR); + +#ifdef B3_DEBUG + { + int totalNumInstances= 0; + for (int k=0;km_numGraphicsInstances; + } + b3Assert(m_data->m_totalNumInstances == totalNumInstances); + } +#endif//B3_DEBUG + + int POSITION_BUFFER_SIZE = (m_data->m_totalNumInstances*sizeof(float)*4); + int ORIENTATION_BUFFER_SIZE = (m_data->m_totalNumInstances*sizeof(float)*4); + int COLOR_BUFFER_SIZE = (m_data->m_totalNumInstances*sizeof(float)*4); +// int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3); + +#if 1 + glBufferSubData( GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes,m_data->m_totalNumInstances*sizeof(float)*4, + &m_data->m_instance_positions_ptr[0]); + glBufferSubData( GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE,m_data->m_totalNumInstances*sizeof(float)*4, + &m_data->m_instance_quaternion_ptr[0]); + glBufferSubData( GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes+ POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE, m_data->m_totalNumInstances*sizeof(float)*4, + &m_data->m_instance_colors_ptr[0]); + glBufferSubData( GL_ARRAY_BUFFER, m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE,m_data->m_totalNumInstances*sizeof(float)*3, + &m_data->m_instance_scale_ptr[0]); +#else char* orgBase = (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE); if (orgBase) { - int totalNumInstances= 0; - - for (int k=0;km_numGraphicsInstances; - } - - m_data->m_totalNumInstances = totalNumInstances; + + for (int k=0;km_vbo); b3Assert(glGetError() ==GL_NO_ERROR); @@ -1006,7 +1043,7 @@ void GLInstancingRenderer::renderScene() { renderSceneInternal(B3_CREATE_SHADOWMAP_RENDERMODE); - // glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); + //glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE); } else @@ -1075,6 +1112,9 @@ void GLInstancingRenderer::drawPoints(const float* positions, const float color[ void GLInstancingRenderer::drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float lineWidthIn) { + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D,0); + float lineWidth = lineWidthIn; b3Clamp(lineWidth,(float)lineWidthRange[0],(float)lineWidthRange[1]); glLineWidth(lineWidth); @@ -1510,7 +1550,7 @@ b3Assert(glGetError() ==GL_NO_ERROR); int curOffset = 0; //GLuint lastBindTexture = 0; - + for (int i=0;im_numGraphicsInstances=%d\n",gfxObj->m_numGraphicsInstances); printf("indexCount=%d\n",indexCount); */ + glUseProgram(createShadowMapInstancingShader); glUniformMatrix4fv(createShadow_depthMVP, 1, false, &depthMVP[0][0]); glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances); @@ -1667,6 +1708,7 @@ b3Assert(glGetError() ==GL_NO_ERROR); glDisable (GL_BLEND); } glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D,0); break; } default: @@ -1743,4 +1785,10 @@ void GLInstancingRenderer::setRenderFrameBuffer(unsigned int renderFrameBuffer) m_data->m_renderFrameBuffer = (GLuint) renderFrameBuffer; } +int GLInstancingRenderer::getTotalNumInstances() const +{ + return m_data->m_totalNumInstances; +} + + #endif //NO_OPENGL3 diff --git a/examples/OpenGLWindow/GLInstancingRenderer.h b/examples/OpenGLWindow/GLInstancingRenderer.h index 66664b844..135f44133 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.h +++ b/examples/OpenGLWindow/GLInstancingRenderer.h @@ -56,7 +56,7 @@ public: void InitShaders(); void CleanupShaders(); virtual void removeAllInstances(); - + virtual void updateShape(int shapeIndex, const float* vertices); ///vertices must be in the format x,y,z, nx,ny,nz, u,v @@ -95,6 +95,9 @@ public: virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex); virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex); + virtual void writeSingleInstanceScaleToCPU(float* scale, int srcIndex); + virtual void writeSingleInstanceScaleToCPU(double* scale, int srcIndex); + virtual struct GLInstanceRendererInternalData* getInternalData(); @@ -125,6 +128,8 @@ public: virtual int getInstanceCapacity() const; + virtual int getTotalNumInstances() const; + virtual void enableShadowMap(); virtual void enableBlend(bool blend) { diff --git a/examples/OpenGLWindow/SimpleCamera.cpp b/examples/OpenGLWindow/SimpleCamera.cpp index 774a1629a..43729b236 100644 --- a/examples/OpenGLWindow/SimpleCamera.cpp +++ b/examples/OpenGLWindow/SimpleCamera.cpp @@ -62,6 +62,16 @@ void SimpleCamera::setVRCamera(const float viewMat[16], const float projectionMa } } +void SimpleCamera::disableVRCamera() +{ + m_data->m_enableVR = false; +} + +bool SimpleCamera::isVRCamera() const +{ + return m_data->m_enableVR ; +} + static void b3CreateFrustum( float left, diff --git a/examples/OpenGLWindow/SimpleCamera.h b/examples/OpenGLWindow/SimpleCamera.h index 51f802f9b..5a057baf0 100644 --- a/examples/OpenGLWindow/SimpleCamera.h +++ b/examples/OpenGLWindow/SimpleCamera.h @@ -15,6 +15,8 @@ struct SimpleCamera : public CommonCameraInterface virtual void getCameraViewMatrix(float m[16]) const; virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16]); + virtual void disableVRCamera(); + virtual bool isVRCamera() const; virtual void getCameraTargetPosition(float pos[3]) const; virtual void getCameraPosition(float pos[3]) const; diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp index 74c701aad..050ec447e 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp @@ -64,6 +64,20 @@ void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(double* color, int src { } + +void SimpleOpenGL2Renderer::writeSingleInstanceScaleToCPU(float* scale, int srcIndex) +{ +} +void SimpleOpenGL2Renderer::writeSingleInstanceScaleToCPU(double* scale, int srcIndex) +{ +} + + +int SimpleOpenGL2Renderer::getTotalNumInstances() const +{ + return 0; +} + void SimpleOpenGL2Renderer::getCameraViewMatrix(float viewMat[16]) const { b3Assert(0); diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h index 5fcd08d3e..393cc7375 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h +++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h @@ -32,6 +32,8 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex); virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex); + virtual void writeSingleInstanceScaleToCPU(float* scale, int srcIndex); + virtual void writeSingleInstanceScaleToCPU(double* scale, int srcIndex); virtual void getCameraViewMatrix(float viewMat[16]) const; virtual void getCameraProjectionMatrix(float projMat[16]) const; @@ -68,6 +70,8 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex); + virtual int getTotalNumInstances() const; + virtual void writeTransforms(); virtual void drawLine(const double from[4], const double to[4], const double color[4], double lineWidth); @@ -82,6 +86,7 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface virtual void clearZBuffer(); + virtual struct GLInstanceRendererInternalData* getInternalData() { return 0; diff --git a/examples/OpenGLWindow/Win32Window.cpp b/examples/OpenGLWindow/Win32Window.cpp index 04ed14493..4c8b600c1 100644 --- a/examples/OpenGLWindow/Win32Window.cpp +++ b/examples/OpenGLWindow/Win32Window.cpp @@ -435,11 +435,10 @@ void Win32Window::setWindowTitle(const char* titleChar) wchar_t windowTitle[1024]; swprintf(windowTitle, 1024, L"%hs", titleChar); - DWORD dwResult; - #ifdef _WIN64 SetWindowTextW(m_data->m_hWnd, windowTitle); #else + DWORD dwResult; SendMessageTimeoutW(m_data->m_hWnd, WM_SETTEXT, 0, reinterpret_cast(windowTitle), SMTO_ABORTIFHUNG, 2000, &dwResult); diff --git a/examples/RenderingExamples/CoordinateSystemDemo.cpp b/examples/RenderingExamples/CoordinateSystemDemo.cpp index 37ecc7f30..7f35af853 100644 --- a/examples/RenderingExamples/CoordinateSystemDemo.cpp +++ b/examples/RenderingExamples/CoordinateSystemDemo.cpp @@ -156,7 +156,7 @@ public: } }; -struct CommonExampleInterface* CoordinateSystemCreateFunc(struct CommonExampleOptions& options) +CommonExampleInterface* CoordinateSystemCreateFunc(struct CommonExampleOptions& options) { return new CoordinateSystemDemo(options.m_guiHelper->getAppInterface()); } diff --git a/examples/RenderingExamples/RaytracerSetup.cpp b/examples/RenderingExamples/RaytracerSetup.cpp index 2aba3f32f..3e328a811 100644 --- a/examples/RenderingExamples/RaytracerSetup.cpp +++ b/examples/RenderingExamples/RaytracerSetup.cpp @@ -295,7 +295,7 @@ void RaytracerPhysicsSetup::stepSimulation(float deltaTime) for (x=0;xm_width;x++) { - for (int y=0;ym_height;y++) + for (y=0;ym_height;y++) { btVector4 rgba(0,0,0,0); btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp new file mode 100644 index 000000000..b0f007940 --- /dev/null +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -0,0 +1,164 @@ + +#include "R2D2GraspExample.h" + +#include "../CommonInterfaces/CommonGraphicsAppInterface.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "../CommonInterfaces/CommonRenderInterface.h" +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../SharedMemory/PhysicsServerSharedMemory.h" +#include "../SharedMemory/PhysicsClientC_API.h" +#include + +#include "b3RobotSimAPI.h" +#include "../Utils/b3Clock.h" + +///quick demo showing the right-handed coordinate system and positive rotations around each axis +class R2D2GraspExample : public CommonExampleInterface +{ + CommonGraphicsApp* m_app; + GUIHelperInterface* m_guiHelper; + b3RobotSimAPI m_robotSim; + int m_options; + int m_r2d2Index; + + float m_x; + float m_y; + float m_z; + b3AlignedObjectArray m_movingInstances; + enum + { + numCubesX = 20, + numCubesY = 20 + }; +public: + + R2D2GraspExample(GUIHelperInterface* helper, int options) + :m_app(helper->getAppInterface()), + m_guiHelper(helper), + m_options(options), + m_r2d2Index(-1), + m_x(0), + m_y(0), + m_z(0) + { + m_app->setUpAxis(2); + } + virtual ~R2D2GraspExample() + { + m_app->m_renderer->enableBlend(false); + } + + + virtual void physicsDebugDraw(int debugDrawMode) + { + + } + virtual void initPhysics() + { + bool connected = m_robotSim.connect(m_guiHelper); + b3Printf("robotSim connected = %d",connected); + b3RobotSimLoadURDFArgs args(""); + + if ((m_options & eROBOTIC_LEARN_GRASP)!=0) + { + args.m_urdfFileName = "r2d2.urdf"; + args.m_startPosition.setValue(0,0,.5); + m_r2d2Index = m_robotSim.loadURDF(args); + int numJoints = m_robotSim.getNumJoints(m_r2d2Index); + b3Printf("numJoints = %d",numJoints); + + for (int i=0;im_renderer->renderScene(); + } + + + virtual void physicsDebugDraw() + { + + } + virtual bool mouseMoveCallback(float x,float y) + { + return false; + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return false; + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + + virtual void resetCamera() + { + float dist = 10; + float pitch = 50; + float yaw = 13; + float targetPos[3]={-1,0,-0.3}; + if (m_app->m_renderer && m_app->m_renderer->getActiveCamera()) + { + m_app->m_renderer->getActiveCamera()->setCameraDistance(dist); + m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch); + m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw); + m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]); + } + } + +}; + + +class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options) +{ + return new R2D2GraspExample(options.m_guiHelper, options.m_option); +} + + + diff --git a/examples/RoboticsLearning/R2D2GraspExample.h b/examples/RoboticsLearning/R2D2GraspExample.h new file mode 100644 index 000000000..353f684e5 --- /dev/null +++ b/examples/RoboticsLearning/R2D2GraspExample.h @@ -0,0 +1,28 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2016 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef R2D2_GRASP_EXAMPLE_H +#define R2D2_GRASP_EXAMPLE_H + +enum RobotLearningExampleOptions +{ + eROBOTIC_LEARN_GRASP=1, + eROBOTIC_LEARN_COMPLIANT_CONTACT=2, +}; + +class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options); + + +#endif //R2D2_GRASP_EXAMPLE_H diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp new file mode 100644 index 000000000..3310d82ce --- /dev/null +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -0,0 +1,729 @@ +#include "b3RobotSimAPI.h" + +//#include "../CommonInterfaces/CommonGraphicsAppInterface.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "../CommonInterfaces/CommonRenderInterface.h" +//#include "../CommonInterfaces/CommonExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../SharedMemory/PhysicsServerSharedMemory.h" +#include "../SharedMemory/PhysicsClientC_API.h" +#include + + + + + + +#include "../Utils/b3Clock.h" +#include "../MultiThreading/b3ThreadSupportInterface.h" + + +void RobotThreadFunc(void* userPtr,void* lsMemory); +void* RobotlsMemoryFunc(); +#define MAX_ROBOT_NUM_THREADS 1 +enum + { + numCubesX = 20, + numCubesY = 20 + }; + + +enum TestRobotSimCommunicationEnums +{ + eRequestTerminateRobotSim= 13, + eRobotSimIsUnInitialized, + eRobotSimIsInitialized, + eRobotSimInitializationFailed, + eRobotSimHasTerminated +}; + +enum MultiThreadedGUIHelperCommunicationEnums +{ + eRobotSimGUIHelperIdle= 13, + eRobotSimGUIHelperRegisterTexture, + eRobotSimGUIHelperRegisterGraphicsShape, + eRobotSimGUIHelperRegisterGraphicsInstance, + eRobotSimGUIHelperCreateCollisionShapeGraphicsObject, + eRobotSimGUIHelperCreateCollisionObjectGraphicsObject, + eRobotSimGUIHelperRemoveAllGraphicsInstances, +}; + +#include +//#include "BulletMultiThreaded/PlatformDefinitions.h" + +#ifndef _WIN32 +#include "../MultiThreading/b3PosixThreadSupport.h" + +b3ThreadSupportInterface* createRobotSimThreadSupport(int numThreads) +{ + b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("RobotSimThreads", + RobotThreadFunc, + RobotlsMemoryFunc, + numThreads); + b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo); + + return threadSupport; + +} + + +#elif defined( _WIN32) +#include "../MultiThreading/b3Win32ThreadSupport.h" + +b3ThreadSupportInterface* createRobotSimThreadSupport(int numThreads) +{ + b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("RobotSimThreads",RobotThreadFunc,RobotlsMemoryFunc,numThreads); + b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo); + return threadSupport; + +} +#endif + + + +struct RobotSimArgs +{ + RobotSimArgs() + :m_physicsServerPtr(0) + { + } + b3CriticalSection* m_cs; + + PhysicsServerSharedMemory* m_physicsServerPtr; + b3AlignedObjectArray m_positions; +}; + +struct RobotSimThreadLocalStorage +{ + int threadId; +}; + + +void RobotThreadFunc(void* userPtr,void* lsMemory) +{ + printf("RobotThreadFunc thread started\n"); + RobotSimThreadLocalStorage* localStorage = (RobotSimThreadLocalStorage*) lsMemory; + + RobotSimArgs* args = (RobotSimArgs*) userPtr; + int workLeft = true; + b3Clock clock; + clock.reset(); + bool init = true; + if (init) + { + + args->m_cs->lock(); + args->m_cs->setSharedParam(0,eRobotSimIsInitialized); + args->m_cs->unlock(); + + do + { +//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread? +#if 0 + double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.; + if (deltaTimeInSeconds<(1./260.)) + { + if (deltaTimeInSeconds<.001) + continue; + } + + clock.reset(); +#endif // + args->m_physicsServerPtr->processClientCommands(); + + } while (args->m_cs->getSharedParam(0)!=eRequestTerminateRobotSim); + } else + { + args->m_cs->lock(); + args->m_cs->setSharedParam(0,eRobotSimInitializationFailed); + args->m_cs->unlock(); + } + //do nothing +} + + + +void* RobotlsMemoryFunc() +{ + //don't create local store memory, just return 0 + return new RobotSimThreadLocalStorage; +} + + + +ATTRIBUTE_ALIGNED16(class) MultiThreadedOpenGLGuiHelper : public GUIHelperInterface +{ + CommonGraphicsApp* m_app; + + b3CriticalSection* m_cs; + + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + GUIHelperInterface* m_childGuiHelper; + + const unsigned char* m_texels; + int m_textureWidth; + int m_textureHeight; + + + int m_shapeIndex; + const float* m_position; + const float* m_quaternion; + const float* m_color; + const float* m_scaling; + + const float* m_vertices; + int m_numvertices; + const int* m_indices; + int m_numIndices; + int m_primitiveType; + int m_textureId; + int m_instanceId; + + + MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper) + :m_app(app) + ,m_cs(0), + m_texels(0), + m_textureId(-1) + { + m_childGuiHelper = guiHelper;; + + } + + virtual ~MultiThreadedOpenGLGuiHelper() + { + delete m_childGuiHelper; + } + + void setCriticalSection(b3CriticalSection* cs) + { + m_cs = cs; + } + + b3CriticalSection* getCriticalSection() + { + return m_cs; + } + + virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color){} + + btCollisionObject* m_obj; + btVector3 m_color2; + + virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color) + { + m_obj = obj; + m_color2 = color; + m_cs->lock(); + m_cs->setSharedParam(1,eRobotSimGUIHelperCreateCollisionObjectGraphicsObject); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) + { + } + + } + + btCollisionShape* m_colShape; + virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape) + { + m_colShape = collisionShape; + m_cs->lock(); + m_cs->setSharedParam(1,eRobotSimGUIHelperCreateCollisionShapeGraphicsObject); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) + { + } + + } + + virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld) + { + //this check is to prevent a crash, in case we removed all graphics instances, but there are still physics objects. + //the check will be obsolete, once we have a better/safer way of synchronizing physics->graphics transforms + if ( m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0) + { + m_childGuiHelper->syncPhysicsToGraphics(rbWorld); + } + } + + virtual void render(const btDiscreteDynamicsWorld* rbWorld) + { + m_childGuiHelper->render(0); + } + + virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){} + + virtual int registerTexture(const unsigned char* texels, int width, int height) + { + m_texels = texels; + m_textureWidth = width; + m_textureHeight = height; + + m_cs->lock(); + m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterTexture); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) + { + } + return m_textureId; + } + virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) + { + m_vertices = vertices; + m_numvertices = numvertices; + m_indices = indices; + m_numIndices = numIndices; + m_primitiveType = primitiveType; + m_textureId = textureId; + + m_cs->lock(); + m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterGraphicsShape); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) + { + } + return m_shapeIndex; + } + virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) + { + m_shapeIndex = shapeIndex; + m_position = position; + m_quaternion = quaternion; + m_color = color; + m_scaling = scaling; + + m_cs->lock(); + m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterGraphicsInstance); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) + { + } + return m_instanceId; + } + + virtual void removeAllGraphicsInstances() + { + m_cs->lock(); + m_cs->setSharedParam(1,eRobotSimGUIHelperRemoveAllGraphicsInstances); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) + { + } + } + + virtual Common2dCanvasInterface* get2dCanvasInterface() + { + return 0; + } + + virtual CommonParameterInterface* getParameterInterface() + { + return 0; + } + + virtual CommonRenderInterface* getRenderInterface() + { + return 0; + } + + virtual CommonGraphicsApp* getAppInterface() + { + return m_childGuiHelper->getAppInterface(); + } + + + virtual void setUpAxis(int axis) + { + m_childGuiHelper->setUpAxis(axis); + } + virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ) + { + } + + virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* width, int* height, int* numPixelsCopied) + { + if (width) + *width = 0; + if (height) + *height = 0; + if (numPixelsCopied) + *numPixelsCopied = 0; + } + + virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) + { + } + + virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size) + { + } +}; + + + + + + + + +struct b3RobotSimAPI_InternalData +{ + //GUIHelperInterface* m_guiHelper; + PhysicsServerSharedMemory m_physicsServer; + b3PhysicsClientHandle m_physicsClient; + + b3ThreadSupportInterface* m_threadSupport; + RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS]; + MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper; + + bool m_connected; + + b3RobotSimAPI_InternalData() + :m_multiThreadedHelper(0), + m_physicsClient(0), + m_connected(false) + { + } +}; + +b3RobotSimAPI::b3RobotSimAPI() +{ + m_data = new b3RobotSimAPI_InternalData; +} + +void b3RobotSimAPI::stepSimulation() +{ + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3Assert(b3CanSubmitCommand(m_data->m_physicsClient)); + if (b3CanSubmitCommand(m_data->m_physicsClient)) + { + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, b3InitStepSimulationCommand(m_data->m_physicsClient)); + statusType = b3GetStatusType(statusHandle); + b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED); + } +} + +void b3RobotSimAPI::setGravity(const b3Vector3& gravityAcceleration) +{ + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); + b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); + +} + +b3RobotSimAPI::~b3RobotSimAPI() +{ + delete m_data; +} + +void b3RobotSimAPI::processMultiThreadedGraphicsRequests() +{ + switch (m_data->m_multiThreadedHelper->getCriticalSection()->getSharedParam(1)) + { + case eRobotSimGUIHelperCreateCollisionShapeGraphicsObject: + { + m_data->m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_data->m_multiThreadedHelper->m_colShape); + m_data->m_multiThreadedHelper->getCriticalSection()->lock(); + m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); + m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); + + break; + } + case eRobotSimGUIHelperCreateCollisionObjectGraphicsObject: + { + m_data->m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_data->m_multiThreadedHelper->m_obj, + m_data->m_multiThreadedHelper->m_color2); + m_data->m_multiThreadedHelper->getCriticalSection()->lock(); + m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); + m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } + case eRobotSimGUIHelperRegisterTexture: + { + + m_data->m_multiThreadedHelper->m_textureId = m_data->m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_data->m_multiThreadedHelper->m_texels, + m_data->m_multiThreadedHelper->m_textureWidth,m_data->m_multiThreadedHelper->m_textureHeight); + + m_data->m_multiThreadedHelper->getCriticalSection()->lock(); + m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); + m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); + + break; + } + case eRobotSimGUIHelperRegisterGraphicsShape: + { + m_data->m_multiThreadedHelper->m_shapeIndex = m_data->m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape( + m_data->m_multiThreadedHelper->m_vertices, + m_data->m_multiThreadedHelper->m_numvertices, + m_data->m_multiThreadedHelper->m_indices, + m_data->m_multiThreadedHelper->m_numIndices, + m_data->m_multiThreadedHelper->m_primitiveType, + m_data->m_multiThreadedHelper->m_textureId); + + m_data->m_multiThreadedHelper->getCriticalSection()->lock(); + m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); + m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } + case eRobotSimGUIHelperRegisterGraphicsInstance: + { + m_data->m_multiThreadedHelper->m_instanceId = m_data->m_multiThreadedHelper->m_childGuiHelper->registerGraphicsInstance( + m_data->m_multiThreadedHelper->m_shapeIndex, + m_data->m_multiThreadedHelper->m_position, + m_data->m_multiThreadedHelper->m_quaternion, + m_data->m_multiThreadedHelper->m_color, + m_data->m_multiThreadedHelper->m_scaling); + + m_data->m_multiThreadedHelper->getCriticalSection()->lock(); + m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); + m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } + case eRobotSimGUIHelperRemoveAllGraphicsInstances: + { + m_data->m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances(); + int numRenderInstances = m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances(); + b3Assert(numRenderInstances==0); + + m_data->m_multiThreadedHelper->getCriticalSection()->lock(); + m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); + m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } + case eRobotSimGUIHelperIdle: + default: + { + + } + } + + + + + #if 0 + if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK) + { + btClock rtc; + btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800); + + while (rtc.getTimeMilliseconds() 0)) + { + statusHandle =b3ProcessServerStatus(physClient); + processMultiThreadedGraphicsRequests(); + } + return (b3SharedMemoryStatusHandle) statusHandle; +} + +int b3RobotSimAPI::getNumJoints(int bodyUniqueId) const +{ + return b3GetNumJoints(m_data->m_physicsClient,bodyUniqueId); +} + +bool b3RobotSimAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo) +{ + return (b3GetJointInfo(m_data->m_physicsClient,bodyUniqueId, jointIndex,jointInfo)!=0); +} + +void b3RobotSimAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3JointMotorArgs& args) +{ + b3SharedMemoryStatusHandle statusHandle; + switch (args.m_controlMode) + { + case CONTROL_MODE_VELOCITY: + { + b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_data->m_physicsClient, CONTROL_MODE_VELOCITY); + b3JointInfo jointInfo; + b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo); + int uIndex = jointInfo.m_uIndex; + b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); + b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); + break; + } + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_data->m_physicsClient, CONTROL_MODE_POSITION_VELOCITY_PD); + b3JointInfo jointInfo; + b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo); + int uIndex = jointInfo.m_uIndex; + int qIndex = jointInfo.m_qIndex; + + b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition); + b3JointControlSetKp(command,uIndex,args.m_kp); + b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); + b3JointControlSetKd(command,uIndex,args.m_kd); + b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); + break; + } + case CONTROL_MODE_TORQUE: + { + b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_data->m_physicsClient, CONTROL_MODE_TORQUE); + b3JointInfo jointInfo; + b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo); + int uIndex = jointInfo.m_uIndex; + b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); + break; + } + default: + { + b3Error("Unknown control command in b3RobotSimAPI::setJointMotorControl"); + } + } +} + +int b3RobotSimAPI::loadURDF(const b3RobotSimLoadURDFArgs& args) +{ + int robotUniqueId = -1; + b3Assert(m_data->m_connected); + + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClient, args.m_urdfFileName.c_str()); + + //setting the initial position, orientation and other arguments are optional + + b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0], + args.m_startPosition[1], + args.m_startPosition[2]); + b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0] + ,args.m_startOrientation[1] + ,args.m_startOrientation[2] + ,args.m_startOrientation[3]); + if (args.m_forceOverrideFixedBase) + { + b3LoadUrdfCommandSetUseFixedBase(command,true); + } + statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command); + statusType = b3GetStatusType(statusHandle); + b3Assert(statusType==CMD_URDF_LOADING_COMPLETED); + robotUniqueId = b3GetStatusBodyIndex(statusHandle); + } + + return robotUniqueId; +} + +bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper) +{ + m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper); + + MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper); + + + + + m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS); + + for (int i=0;im_threadSupport->getNumTasks();i++) + { + RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*) m_data->m_threadSupport->getThreadLocalMemory(i); + b3Assert(storage); + storage->threadId = i; + //storage->m_sharedMem = data->m_sharedMem; + } + + + + + for (int w=0;wm_args[w].m_cs = m_data->m_threadSupport->createCriticalSection(); + m_data->m_args[w].m_cs->setSharedParam(0,eRobotSimIsUnInitialized); + int numMoving = 0; + m_data->m_args[w].m_positions.resize(numMoving); + m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer; + int index = 0; + + m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &m_data->m_args[w], w); + while (m_data->m_args[w].m_cs->getSharedParam(0)==eRobotSimIsUnInitialized) + { + } + } + + m_data->m_args[0].m_cs->setSharedParam(1,eRobotSimGUIHelperIdle); + m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs); + + m_data->m_connected = m_data->m_physicsServer.connectSharedMemory( m_data->m_multiThreadedHelper); + + b3Assert(m_data->m_connected); + + m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY); + int canSubmit = b3CanSubmitCommand(m_data->m_physicsClient); + b3Assert(canSubmit); + return m_data->m_connected && canSubmit; +} + +void b3RobotSimAPI::disconnect() +{ + + for (int i=0;im_args[i].m_cs->lock(); + m_data->m_args[i].m_cs->setSharedParam(0,eRequestTerminateRobotSim); + m_data->m_args[i].m_cs->unlock(); + } + int numActiveThreads = MAX_ROBOT_NUM_THREADS; + + while (numActiveThreads) + { + int arg0,arg1; + if (m_data->m_threadSupport->isTaskCompleted(&arg0,&arg1,0)) + { + numActiveThreads--; + printf("numActiveThreads = %d\n",numActiveThreads); + + } else + { + } + }; + + printf("stopping threads\n"); + + delete m_data->m_threadSupport; + m_data->m_threadSupport = 0; + + b3DisconnectSharedMemory(m_data->m_physicsClient); + m_data->m_physicsServer.disconnectSharedMemory(true); + m_data->m_connected = false; +} + +void b3RobotSimAPI::renderScene() +{ + if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) + { + m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms(); + } + + m_data->m_physicsServer.renderScene(); +} diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h new file mode 100644 index 000000000..35b56c67f --- /dev/null +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -0,0 +1,81 @@ +#ifndef B3_ROBOT_SIM_API_H +#define B3_ROBOT_SIM_API_H + +///todo: remove those includes from this header +#include "../SharedMemory/PhysicsClientC_API.h" +#include "../SharedMemory/SharedMemoryPublic.h" +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Quaternion.h" +#include + + +struct b3RobotSimLoadURDFArgs +{ + std::string m_urdfFileName; + b3Vector3 m_startPosition; + b3Quaternion m_startOrientation; + bool m_forceOverrideFixedBase; + + + b3RobotSimLoadURDFArgs(const std::string& urdfFileName) + :m_urdfFileName(urdfFileName), + m_startPosition(b3MakeVector3(0,0,0)), + m_startOrientation(b3Quaternion(0,0,0,1)), + m_forceOverrideFixedBase(false) + { + } +}; + +struct b3JointMotorArgs +{ + int m_controlMode; + + double m_targetPosition; + double m_kp; + + double m_targetVelocity; + double m_kd; + + double m_maxTorqueValue; + + b3JointMotorArgs(int controlMode) + :m_controlMode(controlMode), + m_targetPosition(0), + m_kp(0.1), + m_targetVelocity(0), + m_kd(0.1), + m_maxTorqueValue(1000) + { + } +}; + + +class b3RobotSimAPI +{ + struct b3RobotSimAPI_InternalData* m_data; + void processMultiThreadedGraphicsRequests(); + b3SharedMemoryStatusHandle submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle); + +public: + b3RobotSimAPI(); + virtual ~b3RobotSimAPI(); + + bool connect(struct GUIHelperInterface* guiHelper); + void disconnect(); + + int loadURDF(const struct b3RobotSimLoadURDFArgs& args); + + int getNumJoints(int bodyUniqueId) const; + + bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); + + void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3JointMotorArgs& args); + + void stepSimulation(); + + void setGravity(const b3Vector3& gravityAcceleration); + + void renderScene(); +}; + +#endif //B3_ROBOT_SIM_API_H diff --git a/examples/RollingFrictionDemo/RollingFrictionDemo.cpp b/examples/RollingFrictionDemo/RollingFrictionDemo.cpp index 1d3213124..7e53be9b0 100644 --- a/examples/RollingFrictionDemo/RollingFrictionDemo.cpp +++ b/examples/RollingFrictionDemo/RollingFrictionDemo.cpp @@ -234,9 +234,8 @@ void RollingFrictionDemo::initPhysics() { btSerializer* s = new btDefaultSerializer; m_dynamicsWorld->serialize(s); - b3ResourcePath p; char resourcePath[1024]; - if (p.findResourcePath("slope.bullet",resourcePath,1024)) + if (b3ResourcePath::findResourcePath("slope.bullet",resourcePath,1024)) { FILE* f = fopen(resourcePath,"wb"); fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index a4199071a..e8e181992 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -835,7 +835,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime) enqueueCommand(CMD_STEP_FORWARD_SIMULATION); if (m_options != eCLIENTEXAMPLE_SERVER) { - enqueueCommand(CMD_REQUEST_DEBUG_LINES); + //enqueueCommand(CMD_REQUEST_DEBUG_LINES); } } } @@ -855,3 +855,4 @@ class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOpt } return example; } + diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 3ca2aa5c5..fa4362caf 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -659,6 +659,8 @@ bool PhysicsClientSharedMemory::canSubmitCommand() const { } struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() { + static int sequence = 0; + m_data->m_testBlock1->m_clientCommands[0].m_sequenceNumber = sequence++; return &m_data->m_testBlock1->m_clientCommands[0]; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6ead18363..4896bd6f1 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -10,7 +10,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" -#include "../CommonInterfaces/CommonRenderInterface.h" + #include "btBulletDynamicsCommon.h" @@ -1756,10 +1756,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } case CMD_STEP_FORWARD_SIMULATION: { - + if (m_data->m_verboseOutput) { b3Printf("Step simulation request"); + b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber); } ///todo(erwincoumans) move this damping inside Bullet for (int i=0;im_bodyHandles.size();i++) @@ -1864,16 +1865,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm case CMD_RESET_SIMULATION: { //clean up all data - if (m_data && m_data->m_guiHelper && m_data->m_guiHelper->getRenderInterface()) - { - m_data->m_guiHelper->getRenderInterface()->removeAllInstances(); - } + + + + if (m_data && m_data->m_guiHelper) + { + m_data->m_guiHelper->removeAllGraphicsInstances(); + } if (m_data) { m_data->m_visualConverter.resetAll(); } + deleteDynamicsWorld(); createEmptyDynamicsWorld(); + m_data->exitHandles(); m_data->initHandles(); @@ -2058,6 +2064,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } case CMD_APPLY_EXTERNAL_FORCE: { + if (m_data->m_verboseOutput) + { + b3Printf("CMD_APPLY_EXTERNAL_FORCE clientCmd = %d\n", clientCmd.m_sequenceNumber); + } for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i) { InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 05c689e8a..3f35f7cc5 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1,5 +1,6 @@ +//todo(erwincoumans): re-use the upcoming b3RobotSimAPI here #include "PhysicsServerExample.h" @@ -9,11 +10,369 @@ #include "SharedMemoryCommon.h" +#include "../Utils/b3Clock.h" +#include "../MultiThreading/b3ThreadSupportInterface.h" + + +void MotionThreadFunc(void* userPtr,void* lsMemory); +void* MotionlsMemoryFunc(); +#define MAX_MOTION_NUM_THREADS 1 +enum + { + numCubesX = 20, + numCubesY = 20 + }; + + +enum TestExampleBrowserCommunicationEnums +{ + eRequestTerminateMotion= 13, + eMotionIsUnInitialized, + eMotionIsInitialized, + eMotionInitializationFailed, + eMotionHasTerminated +}; + +enum MultiThreadedGUIHelperCommunicationEnums +{ + eGUIHelperIdle= 13, + eGUIHelperRegisterTexture, + eGUIHelperRegisterGraphicsShape, + eGUIHelperRegisterGraphicsInstance, + eGUIHelperCreateCollisionShapeGraphicsObject, + eGUIHelperCreateCollisionObjectGraphicsObject, + eGUIHelperRemoveAllGraphicsInstances, +}; + +#include +//#include "BulletMultiThreaded/PlatformDefinitions.h" + +#ifndef _WIN32 +#include "../MultiThreading/b3PosixThreadSupport.h" + +b3ThreadSupportInterface* createMotionThreadSupport(int numThreads) +{ + b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("MotionThreads", + MotionThreadFunc, + MotionlsMemoryFunc, + numThreads); + b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo); + + return threadSupport; + +} + + +#elif defined( _WIN32) +#include "../MultiThreading/b3Win32ThreadSupport.h" + +b3ThreadSupportInterface* createMotionThreadSupport(int numThreads) +{ + b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("MotionThreads",MotionThreadFunc,MotionlsMemoryFunc,numThreads); + b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo); + return threadSupport; + +} +#endif + + + +struct MotionArgs +{ + MotionArgs() + :m_physicsServerPtr(0) + { + } + b3CriticalSection* m_cs; + + PhysicsServerSharedMemory* m_physicsServerPtr; + b3AlignedObjectArray m_positions; +}; + +struct MotionThreadLocalStorage +{ + int threadId; +}; + +int skip = 0; + +void MotionThreadFunc(void* userPtr,void* lsMemory) +{ + printf("MotionThreadFunc thread started\n"); + MotionThreadLocalStorage* localStorage = (MotionThreadLocalStorage*) lsMemory; + + MotionArgs* args = (MotionArgs*) userPtr; + int workLeft = true; + b3Clock clock; + clock.reset(); + bool init = true; + if (init) + { + + args->m_cs->lock(); + args->m_cs->setSharedParam(0,eMotionIsInitialized); + args->m_cs->unlock(); + + do + { +//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread? +#if 0 + double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.; + if (deltaTimeInSeconds<(1./260.)) + { + skip++; + if (deltaTimeInSeconds<.001) + continue; + } + + clock.reset(); +#endif + args->m_physicsServerPtr->processClientCommands(); + + + } while (args->m_cs->getSharedParam(0)!=eRequestTerminateMotion); + } else + { + args->m_cs->lock(); + args->m_cs->setSharedParam(0,eMotionInitializationFailed); + args->m_cs->unlock(); + } + + + printf("finished, #skip = %d\n",skip); + skip=0; + //do nothing + +} + + + +void* MotionlsMemoryFunc() +{ + //don't create local store memory, just return 0 + return new MotionThreadLocalStorage; +} + + + +class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface +{ + CommonGraphicsApp* m_app; + + b3CriticalSection* m_cs; + + +public: + + GUIHelperInterface* m_childGuiHelper; + + const unsigned char* m_texels; + int m_textureWidth; + int m_textureHeight; + + + int m_shapeIndex; + const float* m_position; + const float* m_quaternion; + const float* m_color; + const float* m_scaling; + + const float* m_vertices; + int m_numvertices; + const int* m_indices; + int m_numIndices; + int m_primitiveType; + int m_textureId; + int m_instanceId; + + + MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper) + :m_app(app) + ,m_cs(0), + m_texels(0), + m_textureId(-1) + { + m_childGuiHelper = guiHelper;; + + } + + virtual ~MultiThreadedOpenGLGuiHelper() + { + delete m_childGuiHelper; + } + + void setCriticalSection(b3CriticalSection* cs) + { + m_cs = cs; + } + + b3CriticalSection* getCriticalSection() + { + return m_cs; + } + + virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color){} + + btCollisionObject* m_obj; + btVector3 m_color2; + + virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color) + { + m_obj = obj; + m_color2 = color; + m_cs->lock(); + m_cs->setSharedParam(1,eGUIHelperCreateCollisionObjectGraphicsObject); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eGUIHelperIdle) + { + } + + } + + btCollisionShape* m_colShape; + virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape) + { + m_colShape = collisionShape; + m_cs->lock(); + m_cs->setSharedParam(1,eGUIHelperCreateCollisionShapeGraphicsObject); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eGUIHelperIdle) + { + } + + } + + virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld) + { + //this check is to prevent a crash, in case we removed all graphics instances, but there are still physics objects. + //the check will be obsolete, once we have a better/safer way of synchronizing physics->graphics transforms + if ( m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0) + { + m_childGuiHelper->syncPhysicsToGraphics(rbWorld); + } + } + + virtual void render(const btDiscreteDynamicsWorld* rbWorld) + { + m_childGuiHelper->render(0); + } + + virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){} + + virtual int registerTexture(const unsigned char* texels, int width, int height) + { + m_texels = texels; + m_textureWidth = width; + m_textureHeight = height; + + m_cs->lock(); + m_cs->setSharedParam(1,eGUIHelperRegisterTexture); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eGUIHelperIdle) + { + } + return m_textureId; + } + virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) + { + m_vertices = vertices; + m_numvertices = numvertices; + m_indices = indices; + m_numIndices = numIndices; + m_primitiveType = primitiveType; + m_textureId = textureId; + + m_cs->lock(); + m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsShape); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eGUIHelperIdle) + { + } + return m_shapeIndex; + } + virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) + { + m_shapeIndex = shapeIndex; + m_position = position; + m_quaternion = quaternion; + m_color = color; + m_scaling = scaling; + + m_cs->lock(); + m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsInstance); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eGUIHelperIdle) + { + } + return m_instanceId; + } + + virtual void removeAllGraphicsInstances() + { + m_cs->lock(); + m_cs->setSharedParam(1,eGUIHelperRemoveAllGraphicsInstances); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eGUIHelperIdle) + { + } + } + + virtual Common2dCanvasInterface* get2dCanvasInterface() + { + return 0; + } + + virtual CommonParameterInterface* getParameterInterface() + { + return 0; + } + + virtual CommonRenderInterface* getRenderInterface() + { + return 0; + } + + virtual CommonGraphicsApp* getAppInterface() + { + return m_childGuiHelper->getAppInterface(); + } + + + virtual void setUpAxis(int axis) + { + m_childGuiHelper->setUpAxis(axis); + } + virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ) + { + } + + virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* width, int* height, int* numPixelsCopied) + { + if (width) + *width = 0; + if (height) + *height = 0; + if (numPixelsCopied) + *numPixelsCopied = 0; + } + + virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) + { + } + + virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size) + { + } +}; + + class PhysicsServerExample : public SharedMemoryCommon { PhysicsServerSharedMemory m_physicsServer; - + b3ThreadSupportInterface* m_threadSupport; + MotionArgs m_args[MAX_MOTION_NUM_THREADS]; + MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper; bool m_wantsShutdown; bool m_isConnected; @@ -23,7 +382,7 @@ class PhysicsServerExample : public SharedMemoryCommon public: - PhysicsServerExample(GUIHelperInterface* helper, SharedMemoryInterface* sharedMem=0, int options=0); + PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem=0, int options=0); virtual ~PhysicsServerExample(); @@ -56,7 +415,7 @@ public: virtual bool wantsTermination(); virtual bool isConnected(); virtual void renderScene(); - virtual void exitPhysics(){} + virtual void exitPhysics(); virtual void physicsDebugDraw(int debugFlags); @@ -71,7 +430,6 @@ public: if (!renderer) { - btAssert(0); return false; } @@ -91,7 +449,6 @@ public: if (!renderer) { - btAssert(0); return false; } @@ -134,7 +491,7 @@ public: }; -PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper, SharedMemoryInterface* sharedMem, int options) +PhysicsServerExample::PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem, int options) :SharedMemoryCommon(helper), m_physicsServer(sharedMem), m_wantsShutdown(false), @@ -142,6 +499,7 @@ m_isConnected(false), m_replay(false), m_options(options) { + m_multiThreadedHelper = helper; b3Printf("Started PhysicsServer\n"); } @@ -165,22 +523,80 @@ void PhysicsServerExample::initPhysics() int upAxis = 2; m_guiHelper->setUpAxis(upAxis); -#if 0 - createEmptyDynamicsWorld(); + - //todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire - btVector3 grav(0,0,0); - grav[upAxis] = 0;//-9.8; - this->m_dynamicsWorld->setGravity(grav); -#endif + m_threadSupport = createMotionThreadSupport(MAX_MOTION_NUM_THREADS); + + - m_isConnected = m_physicsServer.connectSharedMemory( m_guiHelper); + for (int i=0;igetNumTasks();i++) + { + MotionThreadLocalStorage* storage = (MotionThreadLocalStorage*) m_threadSupport->getThreadLocalMemory(i); + b3Assert(storage); + storage->threadId = i; + //storage->m_sharedMem = data->m_sharedMem; + } + + + + for (int w=0;wcreateCriticalSection(); + m_args[w].m_cs->setSharedParam(0,eMotionIsUnInitialized); + int numMoving = 0; + m_args[w].m_positions.resize(numMoving); + m_args[w].m_physicsServerPtr = &m_physicsServer; + int index = 0; + + m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &this->m_args[w], w); + while (m_args[w].m_cs->getSharedParam(0)==eMotionIsUnInitialized) + { + } + } + + m_args[0].m_cs->setSharedParam(1,eGUIHelperIdle); + m_multiThreadedHelper->setCriticalSection(m_args[0].m_cs); + m_isConnected = m_physicsServer.connectSharedMemory( m_guiHelper); } +void PhysicsServerExample::exitPhysics() +{ + for (int i=0;ilock(); + m_args[i].m_cs->setSharedParam(0,eRequestTerminateMotion); + m_args[i].m_cs->unlock(); + } + int numActiveThreads = MAX_MOTION_NUM_THREADS; + + while (numActiveThreads) + { + int arg0,arg1; + if (m_threadSupport->isTaskCompleted(&arg0,&arg1,0)) + { + numActiveThreads--; + printf("numActiveThreads = %d\n",numActiveThreads); + + } else + { + } + }; + + printf("stopping threads\n"); + + delete m_threadSupport; + m_threadSupport = 0; + + //m_physicsServer.resetDynamicsWorld(); + +} + + + bool PhysicsServerExample::wantsTermination() { return m_wantsShutdown; @@ -190,6 +606,93 @@ bool PhysicsServerExample::wantsTermination() void PhysicsServerExample::stepSimulation(float deltaTime) { + //this->m_physicsServer.processClientCommands(); + + //check if any graphics related tasks are requested + + switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1)) + { + case eGUIHelperCreateCollisionShapeGraphicsObject: + { + m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_multiThreadedHelper->m_colShape); + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + + break; + } + case eGUIHelperCreateCollisionObjectGraphicsObject: + { + m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_multiThreadedHelper->m_obj, + m_multiThreadedHelper->m_color2); + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } + case eGUIHelperRegisterTexture: + { + + m_multiThreadedHelper->m_textureId = m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_multiThreadedHelper->m_texels, + m_multiThreadedHelper->m_textureWidth,m_multiThreadedHelper->m_textureHeight); + + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + + break; + } + case eGUIHelperRegisterGraphicsShape: + { + m_multiThreadedHelper->m_shapeIndex = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape( + m_multiThreadedHelper->m_vertices, + m_multiThreadedHelper->m_numvertices, + m_multiThreadedHelper->m_indices, + m_multiThreadedHelper->m_numIndices, + m_multiThreadedHelper->m_primitiveType, + m_multiThreadedHelper->m_textureId); + + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } + case eGUIHelperRegisterGraphicsInstance: + { + m_multiThreadedHelper->m_instanceId = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsInstance( + m_multiThreadedHelper->m_shapeIndex, + m_multiThreadedHelper->m_position, + m_multiThreadedHelper->m_quaternion, + m_multiThreadedHelper->m_color, + m_multiThreadedHelper->m_scaling); + + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } + case eGUIHelperRemoveAllGraphicsInstances: + { + m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances(); + int numRenderInstances = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances(); + b3Assert(numRenderInstances==0); + + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } + case eGUIHelperIdle: + default: + { + + } + } + + + + + #if 0 if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK) { btClock rtc; @@ -201,15 +704,59 @@ void PhysicsServerExample::stepSimulation(float deltaTime) } } else { - for (int i=0;i<10;i++) + //for (int i=0;i<10;i++) m_physicsServer.processClientCommands(); } + #endif + + { + if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) + { + m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms(); + } + } } + void PhysicsServerExample::renderScene() { ///debug rendering + //m_args[0].m_cs->lock(); + m_physicsServer.renderScene(); + + if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) + { + //some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift) + + static int frameCount=0; + frameCount++; + char bla[1024]; + sprintf(bla,"VR sub-title text test, frame %d", frameCount/2); + float pos[4]; + m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos); + btTransform viewTr; + btScalar m[16]; + float mf[16]; + m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraViewMatrix(mf); + for (int i=0;i<16;i++) + { + m[i] = mf[i]; + } + viewTr.setFromOpenGLMatrix(m); + btTransform viewTrInv = viewTr.inverse(); + float upMag = -.6; + btVector3 side = viewTrInv.getBasis().getColumn(0); + btVector3 up = viewTrInv.getBasis().getColumn(1); + up+=0.35*side; + m_guiHelper->getAppInterface()->drawText3D(bla,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1); + //btVector3 fwd = viewTrInv.getBasis().getColumn(2); + sprintf(bla,"VR line 2 sub-title text test, frame %d", frameCount/2); + upMag = -0.7; + m_guiHelper->getAppInterface()->drawText3D(bla,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1); + } + + //m_args[0].m_cs->unlock(); } void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags) @@ -290,7 +837,13 @@ extern int gSharedMemoryKey; class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options) { - PhysicsServerExample* example = new PhysicsServerExample(options.m_guiHelper, options.m_sharedMem, options.m_option); + + MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(options.m_guiHelper->getAppInterface(),options.m_guiHelper); + + PhysicsServerExample* example = new PhysicsServerExample(guiHelperWrapper, + options.m_sharedMem, + options.m_option); + if (gSharedMemoryKey>=0) { example->setSharedMemoryKey(gSharedMemoryKey); @@ -306,3 +859,5 @@ class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOpt return example; } + +B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc) \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp index af8101464..0d29fae1e 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp @@ -90,6 +90,11 @@ PhysicsServerSharedMemory::~PhysicsServerSharedMemory() delete m_data; } +void PhysicsServerSharedMemory::resetDynamicsWorld() +{ + m_data->m_commandProcessor->deleteDynamicsWorld(); + m_data->m_commandProcessor ->createEmptyDynamicsWorld(); +} void PhysicsServerSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.h b/examples/SharedMemory/PhysicsServerSharedMemory.h index 3db5ff497..2c08124d0 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.h +++ b/examples/SharedMemory/PhysicsServerSharedMemory.h @@ -43,6 +43,7 @@ public: void enableCommandLogging(bool enable, const char* fileName); void replayFromLogFile(const char* fileName); + void resetDynamicsWorld(); }; diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index 34aea2de5..c2ceb1a05 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -15,7 +15,8 @@ links { language "C++" -files { +myfiles = +{ "PhysicsClient.cpp", "PhysicsClientSharedMemory.cpp", "PhysicsClientExample.cpp", @@ -24,7 +25,6 @@ files { "PhysicsServerSharedMemory.h", "PhysicsServer.cpp", "PhysicsServer.h", - "main.cpp", "PhysicsClientC_API.cpp", "SharedMemoryCommands.h", "SharedMemoryPublic.h", @@ -76,11 +76,211 @@ files { "../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", "../Importers/ImportColladaDemo/ColladaGraphicsInstance.h", "../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp", - "../ThirdPartyLibs/tinyxml/tinystr.cpp", - "../ThirdPartyLibs/tinyxml/tinyxml.cpp", - "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", - "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", -"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", - "../ThirdPartyLibs/stb_image/stb_image.cpp", + "../ThirdPartyLibs/tinyxml/tinystr.cpp", + "../ThirdPartyLibs/tinyxml/tinyxml.cpp", + "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", + "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } +files { + myfiles, + "main.cpp", +} + + +files { + "../MultiThreading/b3ThreadSupportInterface.cpp", + "../MultiThreading/b3ThreadSupportInterface.h" + } + if os.is("Windows") then + + files { + "../MultiThreading/b3Win32ThreadSupport.cpp", + "../MultiThreading/b3Win32ThreadSupport.h" + } + --links {"winmm"} + --defines {"__WINDOWS_MM__", "WIN32"} + end + + if os.is("Linux") then + files { + "../MultiThreading/b3PosixThreadSupport.cpp", + "../MultiThreading/b3PosixThreadSupport.h" + } + + links {"pthread"} + end + + if os.is("MacOSX") then + files { + "../MultiThreading/b3PosixThreadSupport.cpp", + "../MultiThreading/b3PosixThreadSupport.h" + } + + links {"pthread"} + --links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"} + --defines {"__MACOSX_CORE__"} + end + + +project "App_SharedMemoryPhysics_GUI" + +if _OPTIONS["ios"] then + kind "WindowedApp" +else + kind "ConsoleApp" +end +defines {"B3_USE_STANDALONE_EXAMPLE"} + +includedirs {"../../src"} + +links { + "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common" +} + initOpenGL() + initGlew() + +language "C++" + +files { + myfiles, + "../StandaloneMain/main_opengl_single_example.cpp", + "../ExampleBrowser/OpenGLGuiHelper.cpp", + "../ExampleBrowser/GL_ShapeDrawer.cpp", + "../ExampleBrowser/CollisionShape2TriangleMesh.cpp", +} + +if os.is("Linux") then initX11() end + +if os.is("MacOSX") then + links{"Cocoa.framework"} +end + + +files { + "../MultiThreading/b3ThreadSupportInterface.cpp", + "../MultiThreading/b3ThreadSupportInterface.h" + } +if os.is("Windows") then + + files { + "../MultiThreading/b3Win32ThreadSupport.cpp", + "../MultiThreading/b3Win32ThreadSupport.h" + } + --links {"winmm"} + --defines {"__WINDOWS_MM__", "WIN32"} +end + +if os.is("Linux") then + files { + "../MultiThreading/b3PosixThreadSupport.cpp", + "../MultiThreading/b3PosixThreadSupport.h" + } + + links {"pthread"} +end + +if os.is("MacOSX") then + files { + "../MultiThreading/b3PosixThreadSupport.cpp", + "../MultiThreading/b3PosixThreadSupport.h" + } + + links {"pthread"} + --links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"} + --defines {"__MACOSX_CORE__"} +end + +if os.is("Windows") then + project "App_SharedMemoryPhysics_VR" + --for now, only enable VR under Windows, until compilation issues are resolved on Mac/Linux + defines {"B3_USE_STANDALONE_EXAMPLE","BT_ENABLE_VR"} + + if _OPTIONS["ios"] then + kind "WindowedApp" + else + kind "ConsoleApp" + end + + includedirs { + ".","../../src", "../ThirdPartyLibs", + "../ThirdPartyLibs/openvr/headers", + "../ThirdPartyLibs/openvr/samples/shared" + } + + links { + "Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api" + } + + + language "C++" + + + initOpenGL() + initGlew() + + + files + { + myfiles, + "../StandaloneMain/hellovr_opengl_main.cpp", + "../ExampleBrowser/OpenGLGuiHelper.cpp", + "../ExampleBrowser/GL_ShapeDrawer.cpp", + "../ExampleBrowser/CollisionShape2TriangleMesh.cpp", + "../ThirdPartyLibs/openvr/samples/shared/lodepng.cpp", + "../ThirdPartyLibs/openvr/samples/shared/lodepng.h", + "../ThirdPartyLibs/openvr/samples/shared/Matrices.cpp", + "../ThirdPartyLibs/openvr/samples/shared/Matrices.h", + "../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp", + "../ThirdPartyLibs/openvr/samples/shared/pathtools.h", + "../ThirdPartyLibs/openvr/samples/shared/Vectors.h", + } + if os.is("Windows") then + libdirs {"../ThirdPartyLibs/openvr/lib/win32"} + end + + if os.is("Linux") then initX11() end + + if os.is("MacOSX") then + links{"Cocoa.framework"} + end + + + files { + "../MultiThreading/b3ThreadSupportInterface.cpp", + "../MultiThreading/b3ThreadSupportInterface.h" + } + if os.is("Windows") then + + files { + "../MultiThreading/b3Win32ThreadSupport.cpp", + "../MultiThreading/b3Win32ThreadSupport.h" + } + --links {"winmm"} + --defines {"__WINDOWS_MM__", "WIN32"} + end + + if os.is("Linux") then + files { + "../MultiThreading/b3PosixThreadSupport.cpp", + "../MultiThreading/b3PosixThreadSupport.h" + } + + links {"pthread"} + end + + if os.is("MacOSX") then + files { + "../MultiThreading/b3PosixThreadSupport.cpp", + "../MultiThreading/b3PosixThreadSupport.h" + } + + links {"pthread"} + --links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"} + --defines {"__MACOSX_CORE__"} + end + + +end \ No newline at end of file diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index 746f10835..dcb7aa22f 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -11,6 +11,7 @@ #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" +int gSharedMemoryKey = -1; //how can you try typing on a keyboard, without seeing it? //it is pretty funny, to see the desktop in VR! @@ -27,7 +28,7 @@ #include "pathtools.h" CommonExampleInterface* sExample; -OpenGLGuiHelper* sGuiPtr = 0; +GUIHelperInterface* sGuiPtr = 0; #if defined(POSIX) @@ -139,7 +140,7 @@ private: SimpleOpenGL3App* m_app; uint32_t m_nWindowWidth; uint32_t m_nWindowHeight; - + bool m_hasContext; private: // OpenGL bookkeeping int m_iTrackedControllerCount; @@ -234,6 +235,7 @@ private: // OpenGL bookkeeping //----------------------------------------------------------------------------- CMainApplication::CMainApplication( int argc, char *argv[] ) : m_app(NULL) + , m_hasContext(false) , m_nWindowWidth( 1280 ) , m_nWindowHeight( 720 ) , m_unSceneProgramID( 0 ) @@ -373,7 +375,9 @@ bool CMainApplication::BInit() */ m_app = new SimpleOpenGL3App("SimpleOpenGL3App",m_nWindowWidth,m_nWindowHeight,true); + sGuiPtr = new OpenGLGuiHelper(m_app,false); + //sGuiPtr = new DummyGUIHelper; CommonExampleOptions options(sGuiPtr); @@ -556,13 +560,16 @@ void CMainApplication::Shutdown() } m_vecRenderModels.clear(); - if( 1)//m_pContext ) + if( m_hasContext) { - glDebugMessageControl( GL_DONT_CARE, GL_DONT_CARE, GL_DONT_CARE, 0, nullptr, GL_FALSE ); - glDebugMessageCallback(nullptr, nullptr); - glDeleteBuffers(1, &m_glSceneVertBuffer); - glDeleteBuffers(1, &m_glIDVertBuffer); - glDeleteBuffers(1, &m_glIDIndexBuffer); + if (m_glSceneVertBuffer) + { + glDebugMessageControl( GL_DONT_CARE, GL_DONT_CARE, GL_DONT_CARE, 0, nullptr, GL_FALSE ); + glDebugMessageCallback(nullptr, nullptr); + glDeleteBuffers(1, &m_glSceneVertBuffer); + glDeleteBuffers(1, &m_glIDVertBuffer); + glDeleteBuffers(1, &m_glIDIndexBuffer); + } if ( m_unSceneProgramID ) { @@ -1067,6 +1074,7 @@ void CMainApplication::SetupScene() glDisableVertexAttribArray(0); glDisableVertexAttribArray(1); + m_hasContext = true; } @@ -1455,6 +1463,8 @@ void CMainApplication::SetupDistortion() //----------------------------------------------------------------------------- void CMainApplication::RenderStereoTargets() { + sExample->stepSimulation(1./60.); + glClearColor( 0.15f, 0.15f, 0.18f, 1.0f ); // nice background color, but not black glEnable( GL_MULTISAMPLE ); @@ -1462,31 +1472,75 @@ void CMainApplication::RenderStereoTargets() m_app->m_instancingRenderer->init(); + Matrix4 rotYtoZ = rotYtoZ.identity(); + //some Bullet apps (especially robotics related) require Z as up-axis) + if (m_app->getUpAxis()==2) + { + rotYtoZ.rotateX(-90); + } + + RenderScene( vr::Eye_Left ); + // Left Eye { - Matrix4 viewMatLeft = m_mat4eyePosLeft * m_mat4HMDPose; + + Matrix4 viewMatLeft = m_mat4eyePosLeft * m_mat4HMDPose * rotYtoZ; + Matrix4 viewMatCenter = m_mat4HMDPose * rotYtoZ; + //0,1,2,3 + //4,5,6,7, + //8,9,10,11 + //12,13,14,15 + + //m_mat4eyePosLeft.get()[10] + //m_app->m_instancingRenderer->getActiveCamera()->setCameraTargetPosition( + // m_mat4eyePosLeft.get()[3], + // m_mat4eyePosLeft.get()[7], + // m_mat4eyePosLeft.get()[11]); + Matrix4 m; + m = viewMatCenter; + const float* mat = m.invertAffine().get(); + + /*printf("camera:\n,%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f", + mat[0],mat[1],mat[2],mat[3], + mat[4],mat[5],mat[6],mat[7], + mat[8],mat[9],mat[10],mat[11], + mat[12],mat[13],mat[14],mat[15]); + */ + float dist=1; + m_app->m_instancingRenderer->getActiveCamera()->setCameraTargetPosition( + mat[12]-dist*mat[8], + mat[13]-dist*mat[9], + mat[14]-dist*mat[10] + ); + m_app->m_instancingRenderer->getActiveCamera()->setCameraUpVector(mat[0],mat[1],mat[2]); m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatLeft.get(),m_mat4ProjectionLeft.get()); - m_app->m_instancingRenderer->updateCamera(); + m_app->m_instancingRenderer->updateCamera(m_app->getUpAxis()); } glBindFramebuffer( GL_FRAMEBUFFER, leftEyeDesc.m_nRenderFramebufferId ); glViewport(0, 0, m_nRenderWidth, m_nRenderHeight ); + m_app->m_window->startRendering(); - RenderScene( vr::Eye_Left ); - m_app->drawGrid(); - sExample->stepSimulation(1./60.); - sExample->renderScene(); + + + + m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)leftEyeDesc.m_nRenderFramebufferId); - m_app->m_instancingRenderer->renderScene(); + sExample->renderScene(); + //m_app->m_instancingRenderer->renderScene(); + DrawGridData gridUp; + gridUp.upAxis = m_app->getUpAxis(); + m_app->drawGrid(gridUp); + glBindFramebuffer( GL_FRAMEBUFFER, 0 ); @@ -1506,22 +1560,25 @@ void CMainApplication::RenderStereoTargets() // Right Eye - { - Matrix4 viewMatRight = m_mat4eyePosRight * m_mat4HMDPose; - m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatRight.get(),m_mat4ProjectionRight.get()); - m_app->m_instancingRenderer->updateCamera(); - } + RenderScene( vr::Eye_Right ); + { + Matrix4 viewMatRight = m_mat4eyePosRight * m_mat4HMDPose * rotYtoZ; + m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatRight.get(),m_mat4ProjectionRight.get()); + m_app->m_instancingRenderer->updateCamera(m_app->getUpAxis()); + } + glBindFramebuffer( GL_FRAMEBUFFER, rightEyeDesc.m_nRenderFramebufferId ); glViewport(0, 0, m_nRenderWidth, m_nRenderHeight ); m_app->m_window->startRendering(); - RenderScene( vr::Eye_Right ); - m_app->drawGrid(); + + m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)rightEyeDesc.m_nRenderFramebufferId); - - m_app->m_renderer->renderScene(); + //m_app->m_renderer->renderScene(); + sExample->renderScene(); + m_app->drawGrid(gridUp); glBindFramebuffer( GL_FRAMEBUFFER, 0 ); diff --git a/examples/StandaloneMain/main_opengl_single_example.cpp b/examples/StandaloneMain/main_opengl_single_example.cpp index a7410433c..8bf97619d 100644 --- a/examples/StandaloneMain/main_opengl_single_example.cpp +++ b/examples/StandaloneMain/main_opengl_single_example.cpp @@ -17,14 +17,9 @@ subject to the following restrictions: #include "../CommonInterfaces/CommonExampleInterface.h" #include "../CommonInterfaces/CommonGUIHelperInterface.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" +#include "../Utils/b3Clock.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btHashMap.h" - #include "../OpenGLWindow/SimpleOpenGL3App.h" @@ -32,6 +27,7 @@ subject to the following restrictions: #include "../ExampleBrowser/OpenGLGuiHelper.h" CommonExampleInterface* example; +int gSharedMemoryKey=-1; b3MouseMoveCallback prevMouseMoveCallback = 0; static void OnMouseMove( float x, float y) @@ -57,9 +53,23 @@ static void OnMouseDown(int button, int state, float x, float y) { } } +class LessDummyGuiHelper : public DummyGUIHelper +{ + CommonGraphicsApp* m_app; +public: + virtual CommonGraphicsApp* getAppInterface() + { + return m_app; + } + + LessDummyGuiHelper(CommonGraphicsApp* app) + :m_app(app) + { + } +}; int main(int argc, char* argv[]) { - + SimpleOpenGL3App* app = new SimpleOpenGL3App("Bullet Standalone Example",1024,768,true); prevMouseButtonCallback = app->m_window->getMouseButtonCallback(); @@ -69,20 +79,26 @@ int main(int argc, char* argv[]) app->m_window->setMouseMoveCallback((b3MouseMoveCallback)OnMouseMove); OpenGLGuiHelper gui(app,false); - + //LessDummyGuiHelper gui(app); + //DummyGUIHelper gui; + CommonExampleOptions options(&gui); example = StandaloneExampleCreateFunc(options); - example->initPhysics(); + example->initPhysics(); example->resetCamera(); + b3Clock clock; + do { app->m_instancingRenderer->init(); app->m_instancingRenderer->updateCamera(app->getUpAxis()); - example->stepSimulation(1./60.); - + btScalar dtSec = btScalar(clock.getTimeInSeconds()); + example->stepSimulation(dtSec); + clock.reset(); + example->renderScene(); DrawGridData dg; diff --git a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp index 1fa1b0ccd..2ab5d3992 100644 --- a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp +++ b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp @@ -105,14 +105,20 @@ public: } - virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) + + virtual int registerTexture(const unsigned char* texels, int width, int height) + { + return -1; + } + + virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) { - int shapeIndex = OpenGLGuiHelper::registerGraphicsShape(vertices,numvertices,indices,numIndices); + int shapeIndex = OpenGLGuiHelper::registerGraphicsShape(vertices,numvertices,indices,numIndices,primitiveType, textureId); if (shapeIndex>=0) { TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer); - float rgbaColor[4] = {1,1,1,1}; - swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor); + float rgbaColor[4] = {1,1,1,1}; + swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor); //swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices); m_swRenderObjects.insert(shapeIndex,swObj); } diff --git a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp index 565b771e7..d4e0fd77b 100644 --- a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp +++ b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp @@ -37,6 +37,12 @@ static btVector4 sMyColors[4] = //btVector4(1,1,0,1), }; +struct TinyRendererTexture +{ + const unsigned char* m_texels; + int m_width; + int m_height; +}; struct TinyRendererGUIHelper : public GUIHelperInterface { @@ -45,6 +51,7 @@ struct TinyRendererGUIHelper : public GUIHelperInterface btHashMap m_swRenderObjects; btHashMap m_swInstances; + btHashMap m_textures; int m_swWidth; int m_swHeight; @@ -151,7 +158,8 @@ struct TinyRendererGUIHelper : public GUIHelperInterface if (gfxVertices.size() && indices.size()) { - int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size()); + int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size(), + 1,-1); collisionShape->setUserIndex(shapeId); } } @@ -249,18 +257,41 @@ struct TinyRendererGUIHelper : public GUIHelperInterface virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){} - virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) + virtual int registerTexture(const unsigned char* texels, int width, int height) + { + //do we need to make a copy? + int textureId = m_textures.size(); + TinyRendererTexture t; + t.m_texels = texels; + t.m_width = width; + t.m_height = height; + this->m_textures.insert(textureId,t); + return textureId; + } + + virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) { int shapeIndex = m_swRenderObjects.size(); TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer); float rgbaColor[4] = {1,1,1,1}; - swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor); + + //if (textureId>=0) + //{ + // swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor); + //} else + { + swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor); + } //swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices); m_swRenderObjects.insert(shapeIndex,swObj); return shapeIndex; } + virtual void removeAllGraphicsInstances() + { + } + virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) { int colIndex = m_colObjUniqueIndex++; diff --git a/examples/ThirdPartyLibs/openvr/LICENSE b/examples/ThirdPartyLibs/openvr/LICENSE new file mode 100644 index 000000000..ee83337d7 --- /dev/null +++ b/examples/ThirdPartyLibs/openvr/LICENSE @@ -0,0 +1,27 @@ +Copyright (c) 2015, Valve Corporation +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this +list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors +may be used to endorse or promote products derived from this software without +specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/examples/ThirdPartyLibs/openvr/README b/examples/ThirdPartyLibs/openvr/README new file mode 100644 index 000000000..9a8923159 --- /dev/null +++ b/examples/ThirdPartyLibs/openvr/README @@ -0,0 +1,9 @@ +OpenVR is an API and runtime that allows access to VR hardware from multiple +vendors without requiring that applications have specific knowledge of the +hardware they are targeting. This repository is an SDK that contains the API +and samples. The runtime is under SteamVR in Tools on Steam. + +Documentation for the API is available in the wiki: https://github.com/ValveSoftware/openvr/wiki/API-Documentation + +More information on OpenVR and SteamVR can be found on http://steamvr.com + diff --git a/examples/Utils/b3Clock.cpp b/examples/Utils/b3Clock.cpp index a504ba856..6802726d9 100644 --- a/examples/Utils/b3Clock.cpp +++ b/examples/Utils/b3Clock.cpp @@ -166,7 +166,7 @@ unsigned long int b3Clock::getTimeMilliseconds() /// Returns the time in us since the last call to reset or since /// the Clock was created. -unsigned long int b3Clock::getTimeMicroseconds() +unsigned long long int b3Clock::getTimeMicroseconds() { #ifdef B3_USE_WINDOWS_TIMERS LARGE_INTEGER currentTime; @@ -175,14 +175,14 @@ unsigned long int b3Clock::getTimeMicroseconds() m_data->mStartTime.QuadPart; // Compute the number of millisecond ticks elapsed. - unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / + unsigned long long msecTicks = (unsigned long long)(1000 * elapsedTime / m_data->mClockFrequency.QuadPart); // Check for unexpected leaps in the Win32 performance counter. // (This is caused by unexpected data across the PCI to ISA // bridge, aka south bridge. See Microsoft KB274323.) - unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick; - signed long msecOff = (signed long)(msecTicks - elapsedTicks); + unsigned long long elapsedTicks = GetTickCount() - m_data->mStartTick; + signed long long msecOff = (signed long)(msecTicks - elapsedTicks); if (msecOff < -100 || msecOff > 100) { // Adjust the starting time forwards. @@ -197,7 +197,7 @@ unsigned long int b3Clock::getTimeMicroseconds() m_data->mPrevElapsedTime = elapsedTime; // Convert to microseconds. - unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / + unsigned long long usecTicks = (unsigned long)(1000000 * elapsedTime / m_data->mClockFrequency.QuadPart); return usecTicks; @@ -222,3 +222,8 @@ unsigned long int b3Clock::getTimeMicroseconds() #endif } +double b3Clock::getTimeInSeconds() +{ + return double(getTimeMicroseconds()/1.e6); +} + diff --git a/examples/Utils/b3Clock.h b/examples/Utils/b3Clock.h index d30c615a3..cb1c0c29d 100644 --- a/examples/Utils/b3Clock.h +++ b/examples/Utils/b3Clock.h @@ -22,7 +22,12 @@ public: /// Returns the time in us since the last call to reset or since /// the Clock was created. - unsigned long int getTimeMicroseconds(); + unsigned long long int getTimeMicroseconds(); + + /// Returns the time in seconds since the last call to reset or since + /// the Clock was created. + double getTimeInSeconds(); + private: struct b3ClockData* m_data; }; diff --git a/examples/Utils/b3ResourcePath.cpp b/examples/Utils/b3ResourcePath.cpp index fdd546d5e..44952d61a 100644 --- a/examples/Utils/b3ResourcePath.cpp +++ b/examples/Utils/b3ResourcePath.cpp @@ -4,7 +4,7 @@ #include /* _NSGetExecutablePath */ #else #ifdef _WIN32 -#include +#include #else //not Mac, not Windows, let's cross the fingers it is Linux :-) #include diff --git a/examples/Vehicles/Hinge2Vehicle.cpp b/examples/Vehicles/Hinge2Vehicle.cpp index 9d72ad57c..794ebcd7c 100644 --- a/examples/Vehicles/Hinge2Vehicle.cpp +++ b/examples/Vehicles/Hinge2Vehicle.cpp @@ -568,11 +568,6 @@ void Hinge2Vehicle::renderScene() m_guiHelper->render(m_dynamicsWorld); - - - ATTRIBUTE_ALIGNED16(btScalar) m[16]; - int i; - btVector3 wheelColor(1,0,0); btVector3 worldBoundsMin,worldBoundsMax; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 501bcf515..58e4df4c2 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -172,6 +172,8 @@ pybullet_loadURDF(PyObject* self, PyObject* args) &startOrnX,&startOrnY,&startOrnZ, &startOrnW)) return NULL; } + + if (strlen(urdfFileName)) { // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); @@ -185,12 +187,16 @@ pybullet_loadURDF(PyObject* self, PyObject* args) statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType!=CMD_URDF_LOADING_COMPLETED) - { - PyErr_SetString(SpamError, "Cannot load URDF file."); - return NULL; - } + { + PyErr_SetString(SpamError, "Cannot load URDF file."); + return NULL; + } bodyIndex = b3GetStatusBodyIndex(statusHandle); - } + } else + { + PyErr_SetString(SpamError, "Empty filename, method expects 1, 4 or 8 arguments."); + return NULL; + } return PyLong_FromLong(bodyIndex); } @@ -368,8 +374,8 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { case CONTROL_MODE_VELOCITY: { + double kd = gains; b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue); - double kd = gains; b3JointControlSetKd(commandHandle,info.m_uIndex,kd); b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce); break; @@ -383,8 +389,8 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) case CONTROL_MODE_POSITION_VELOCITY_PD: { + double kp = gains; b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue); - double kp = gains; b3JointControlSetKp(commandHandle,info.m_uIndex,kp); b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce); break; @@ -1264,10 +1270,12 @@ static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args) PyErr_SetString(SpamError, "flag has to be either WORLD_FRAME or LINK_FRAME"); return NULL; } - b3SharedMemoryStatusHandle statusHandle; - b3SharedMemoryCommandHandle command = b3ApplyExternalForceCommandInit(sm); - b3ApplyExternalTorque(command,objectUniqueId,-1,torque, flags); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + { + b3SharedMemoryStatusHandle statusHandle; + b3SharedMemoryCommandHandle command = b3ApplyExternalForceCommandInit(sm); + b3ApplyExternalTorque(command,objectUniqueId,-1,torque, flags); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + } } } @@ -1383,12 +1391,13 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args) { double rpy[3]; - sqx = quat[0] * quat[0]; + double sarg; + sqx = quat[0] * quat[0]; sqy = quat[1] * quat[1]; sqz = quat[2] * quat[2]; squ = quat[3] * quat[3]; rpy[0] = atan2(2 * (quat[1]*quat[2] + quat[3]*quat[0]), squ - sqx - sqy + sqz); - double sarg = -2 * (quat[0]*quat[2] - quat[3] * quat[1]); + sarg = -2 * (quat[0]*quat[2] - quat[3] * quat[1]); rpy[1] = sarg <= -1.0 ? -0.5*3.141592538 : (sarg >= 1.0 ? 0.5*3.141592538 : asin(sarg)); rpy[2] = atan2(2 * (quat[0]*quat[1] + quat[3]*quat[2]), squ + sqx - sqy - sqz); { diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h index 78d20a97c..ce9941efb 100644 --- a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h +++ b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h @@ -965,8 +965,6 @@ inline void b3DynamicBvh::rayTestInternal( const b3DbvtNode* root, B3_DBVT_CHECKTYPE if(root) { - b3Vector3 resultNormal; - int depth=1; int treshold=B3_DOUBLE_STACKSIZE-2; b3AlignedObjectArray& stack = m_rayTestStack; diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp index a0cc00496..b7201d2d7 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp @@ -692,8 +692,6 @@ static bool findSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3Con } } - b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend; - int curEdgeEdge = 0; // Test edges for(int e0=0;e0getAngularVelocity(); - if (rb1) - angVelB = rb1->getAngularVelocity(); + if (mbA) + angVelA = mbA->getVelocityVector()>getLink(fcA->m_link).l>getAngularVelocity(); + if (mbB) + angVelB = mbB->getAngularVelocity(); btVector3 relAngVel = angVelB-angVelA; if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index 6bc517c8e..0a8b6031d 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -58,6 +58,7 @@ void testSharedMemory(b3PhysicsClientHandle sm) int statusType; int bodyIndicesOut[10];//MAX_SDF_BODIES = 10 int numJoints, numBodies; + int bodyUniqueId; b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); @@ -65,7 +66,7 @@ void testSharedMemory(b3PhysicsClientHandle sm) numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10); ASSERT_EQ(numBodies,1); - int bodyUniqueId = bodyIndicesOut[0]; + bodyUniqueId = bodyIndicesOut[0]; numJoints = b3GetNumJoints(sm,bodyUniqueId); ASSERT_EQ(numJoints,7); @@ -106,7 +107,7 @@ void testSharedMemory(b3PhysicsClientHandle sm) b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); //setting the initial position, orientation and other arguments are optional - startPosX =0; + startPosX =2; startPosY =0; startPosZ = 1; ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);