diff --git a/examples/CommonInterfaces/CommonGraphicsAppInterface.h b/examples/CommonInterfaces/CommonGraphicsAppInterface.h index 242c63ccd..674807de8 100644 --- a/examples/CommonInterfaces/CommonGraphicsAppInterface.h +++ b/examples/CommonInterfaces/CommonGraphicsAppInterface.h @@ -66,9 +66,9 @@ struct CommonGraphicsApp m_mouseYpos(0.f), m_mouseInitialized(false) { - m_backgroundColorRGB[0] = 0.9; - m_backgroundColorRGB[1] = 0.9; - m_backgroundColorRGB[2] = 1; + m_backgroundColorRGB[0] = 0.7; + m_backgroundColorRGB[1] = 0.7; + m_backgroundColorRGB[2] = 0.8; } virtual ~CommonGraphicsApp() { diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index a3238f358..be280c877 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -123,6 +123,7 @@ static bool enable_experimental_opencl = false; int gDebugDrawFlags = 0; static bool pauseSimulation=false; +static bool singleStepSimulation = false; int midiBaseIndex = 176; extern bool gDisableDeactivation; @@ -227,6 +228,12 @@ void MyKeyboardCallback(int key, int state) { pauseSimulation = !pauseSimulation; } + if (key == 'o' && state) + { + singleStepSimulation = true; + } + + #ifndef NO_OPENGL3 if (key=='s' && state) { @@ -490,7 +497,7 @@ void MyComboBoxCallback(int comboId, const char* item) void MyGuiPrintf(const char* msg) { printf("b3Printf: %s\n",msg); - if (gui2) + if (!gDisableDemoSelection) { gui2->textOutput(msg); gui2->forceUpdateScrollBars(); @@ -502,7 +509,7 @@ void MyGuiPrintf(const char* msg) void MyStatusBarPrintf(const char* msg) { printf("b3Printf: %s\n", msg); - if (gui2) + if (!gDisableDemoSelection) { bool isLeft = true; gui2->setStatusBarMessage(msg,isLeft); @@ -513,7 +520,7 @@ void MyStatusBarPrintf(const char* msg) void MyStatusBarError(const char* msg) { printf("Warning: %s\n", msg); - if (gui2) + if (!gDisableDemoSelection) { bool isLeft = false; gui2->setStatusBarMessage(msg,isLeft); @@ -1124,12 +1131,12 @@ void OpenGLExampleBrowser::update(float deltaTime) if (sCurrentDemo) { - if (!pauseSimulation) + if (!pauseSimulation || singleStepSimulation) { + singleStepSimulation = false; //printf("---------------------------------------------------\n"); //printf("Framecount = %d\n",frameCount); - - + if (gFixedTimeStep>0) { sCurrentDemo->stepSimulation(gFixedTimeStep); diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index d4aa246dc..e49369547 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -84,7 +84,10 @@ public: virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color) { - drawLine(PointOnB,PointOnB+normalOnB,color); + drawLine(PointOnB,PointOnB+normalOnB*distance,color); + btVector3 red(0.3, 1., 0.3); + drawLine(PointOnB, PointOnB + normalOnB*0.01, red); + } diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 84b5fc7db..7db87b26a 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -58,7 +58,7 @@ public: virtual void physicsDebugDraw(int debugDrawMode) { - + m_robotSim.debugDraw(debugDrawMode); } virtual void initPhysics() { @@ -81,7 +81,16 @@ public: slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } - + { + b3RobotSimLoadFileArgs args(""); + b3RobotSimLoadFileResults results; + args.m_fileName = "cube_small.urdf"; + args.m_startPosition.setValue(0, 0, .107); + args.m_startOrientation.setEulerZYX(0, 0, 0); + args.m_useMultiBody = true; + m_robotSim.loadFile(args, results); + } + { b3RobotSimLoadFileArgs args(""); args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf"; @@ -129,15 +138,7 @@ public: } } } - { - b3RobotSimLoadFileArgs args(""); - b3RobotSimLoadFileResults results; - args.m_fileName = "cube_small.urdf"; - args.m_startPosition.setValue(0,0,.107); - args.m_startOrientation.setEulerZYX(0,0,0); - args.m_useMultiBody = false; - m_robotSim.loadFile(args,results); - } + if (1) { b3RobotSimLoadFileArgs args(""); @@ -179,19 +180,20 @@ public: m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); } } - m_robotSim.stepSimulation(); + m_robotSim.stepSimulation(); } virtual void renderScene() { m_robotSim.renderScene(); //m_app->m_renderer->renderScene(); + } virtual void physicsDebugDraw() { - + } virtual bool mouseMoveCallback(float x,float y) { diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 15cc08a21..4c825acab 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -7,15 +7,15 @@ //#include "../CommonInterfaces/CommonExampleInterface.h" #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "../SharedMemory/PhysicsServerSharedMemory.h" +#include "../SharedMemory/PhysicsServerSharedMemory.h" #include "../SharedMemory/PhysicsClientC_API.h" +#include "../SharedMemory/PhysicsDirectC_API.h" +#include "../SharedMemory/PhysicsDirect.h" + #include - - - - - - #include "../Utils/b3Clock.h" + + #include "../MultiThreading/b3ThreadSupportInterface.h" @@ -406,24 +406,27 @@ public: - - - struct b3RobotSimAPI_InternalData { //GUIHelperInterface* m_guiHelper; PhysicsServerSharedMemory m_physicsServer; b3PhysicsClientHandle m_physicsClient; + b3ThreadSupportInterface* m_threadSupport; RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS]; MultiThreadedOpenGLGuiHelper2* m_multiThreadedHelper; + PhysicsDirect* m_clientServerDirect; + bool m_useMultiThreading; bool m_connected; b3RobotSimAPI_InternalData() - :m_multiThreadedHelper(0), + :m_threadSupport(0), + m_multiThreadedHelper(0), + m_clientServerDirect(0), m_physicsClient(0), + m_useMultiThreading(false), m_connected(false) { } @@ -464,6 +467,9 @@ b3RobotSimAPI::~b3RobotSimAPI() void b3RobotSimAPI::processMultiThreadedGraphicsRequests() { + if (0==m_data->m_multiThreadedHelper) + return; + switch (m_data->m_multiThreadedHelper->getCriticalSection()->getSharedParam(1)) { case eRobotSimGUIHelperCreateCollisionShapeGraphicsObject: @@ -563,27 +569,6 @@ void b3RobotSimAPI::processMultiThreadedGraphicsRequests() } } - - - - #if 0 - if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK) - { - btClock rtc; - btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800); - - while (rtc.getTimeMilliseconds()m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper); - - MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper); - - - - - m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS); - - for (int i=0;im_threadSupport->getNumTasks();i++) + if (m_data->m_useMultiThreading) { - RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*) m_data->m_threadSupport->getThreadLocalMemory(i); - b3Assert(storage); - storage->threadId = i; - //storage->m_sharedMem = data->m_sharedMem; - } + m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper); + + MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper); - - 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_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS); + + for (int i = 0; i < m_data->m_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; w < MAX_ROBOT_NUM_THREADS; w++) + { + m_data->m_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); + + bool serverConnected = m_data->m_physicsServer.connectSharedMemory(m_data->m_multiThreadedHelper); + b3Assert(serverConnected); + + m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY); + } + else + { + m_data->m_clientServerDirect = new PhysicsDirect(); + bool connected = m_data->m_clientServerDirect->connect(guiHelper); + m_data->m_physicsClient = (b3PhysicsClientHandle)m_data->m_clientServerDirect; + } - 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; + //client connected + m_data->m_connected = b3CanSubmitCommand(m_data->m_physicsClient); + + b3Assert(m_data->m_connected); + return m_data->m_connected && m_data->m_connected; } void b3RobotSimAPI::disconnect() { - - for (int i=0;im_useMultiThreading) { - m_data->m_args[i].m_cs->lock(); - m_data->m_args[i].m_cs->setSharedParam(0,eRequestTerminateRobotSim); - m_data->m_args[i].m_cs->unlock(); + for (int i = 0; i < MAX_ROBOT_NUM_THREADS; i++) + { + m_data->m_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; } - 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::debugDraw(int debugDrawMode) +{ + if (m_data->m_clientServerDirect) + { + m_data->m_clientServerDirect->debugDraw(debugDrawMode); + } +} void b3RobotSimAPI::renderScene() { - if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) + if (m_data->m_useMultiThreading) { - m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms(); + if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) + { + m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms(); + } } + if (m_data->m_clientServerDirect) + { + m_data->m_clientServerDirect->renderScene(); + } m_data->m_physicsServer.renderScene(); } diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 9a4a14831..d435b2925 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -95,6 +95,7 @@ public: void setGravity(const b3Vector3& gravityAcceleration); void renderScene(); + void debugDraw(int debugDrawMode); }; #endif //B3_ROBOT_SIM_API_H diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 05c8c0eed..948b90b90 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -69,9 +69,27 @@ PhysicsDirect::~PhysicsDirect() bool PhysicsDirect::connect() { m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx); + return true; } +// return true if connection succesfull, can also check 'isConnected' +bool PhysicsDirect::connect(struct GUIHelperInterface* guiHelper) +{ + m_data->m_commandProcessor->setGuiHelper(guiHelper); + + return true; +} + +void PhysicsDirect::renderScene() +{ + m_data->m_commandProcessor->renderScene(); +} +void PhysicsDirect::debugDraw(int debugDrawMode) +{ + m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode); +} + ////todo: rename to 'disconnect' void PhysicsDirect::disconnectSharedMemory() { diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index aaa2d8639..7e3ae2c94 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -29,9 +29,10 @@ public: virtual ~PhysicsDirect(); - // return true if connection succesfull, can also check 'isConnected' + // return true if connection succesfull, can also check 'isConnected' + //it is OK to pass a null pointer for the gui helper virtual bool connect(); - + ////todo: rename to 'disconnect' virtual void disconnectSharedMemory(); @@ -62,7 +63,12 @@ public: virtual const float* getDebugLinesColor() const; virtual void getCachedCameraImage(b3CameraImageData* cameraData); - + + //those 2 APIs are for internal use for visualization + virtual bool connect(struct GUIHelperInterface* guiHelper); + virtual void renderScene(); + virtual void debugDraw(int debugDrawMode); + }; #endif //PHYSICS_DIRECT_H diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b0adbd12d..96dc083a7 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2445,7 +2445,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size()); } - m_data->m_dynamicsWorld->stepSimulation(dtInSec); + m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,1./240.); } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 59e81ac4c..489d9526f 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -872,7 +872,22 @@ void PhysicsServerExample::renderScene() static int frameCount=0; frameCount++; char bla[1024]; - sprintf(bla,"VR sub-title text test, frame %d", frameCount/2); + + static btScalar prevTime = m_clock.getTimeSeconds(); + btScalar curTime = m_clock.getTimeSeconds(); + static btScalar deltaTime = 0.f; + static int count = 10; + if (count-- < 0) + { + count = 10; + deltaTime = curTime - prevTime; + } + if (deltaTime == 0) + deltaTime = 1000; + + prevTime = curTime; + + sprintf(bla,"VR sub-title text test,fps = %f, frame %d", 1./deltaTime, frameCount/2); float pos[4]; m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos); btTransform viewTr; diff --git a/src/LinearMath/btIDebugDraw.h b/src/LinearMath/btIDebugDraw.h index 58c9838c4..a020c3f4e 100644 --- a/src/LinearMath/btIDebugDraw.h +++ b/src/LinearMath/btIDebugDraw.h @@ -166,9 +166,9 @@ class btIDebugDraw virtual void drawTransform(const btTransform& transform, btScalar orthoLen) { btVector3 start = transform.getOrigin(); - drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0)); - drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0)); - drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f)); + drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(1.f,0.3,0.3)); + drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0.3,1.f, 0.3)); + drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0.3, 0.3,1.f)); } virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,