From f72982306ea3c12ae181630bbf14a54670581262 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Fri, 9 Sep 2016 11:28:38 -0700 Subject: [PATCH] fix issues with VR physics server --- .../PhysicsServerCommandProcessor.cpp | 28 +++++--- .../SharedMemory/PhysicsServerExample.cpp | 70 +++++++++++++------ .../StandaloneMain/hellovr_opengl_main.cpp | 8 +-- 3 files changed, 69 insertions(+), 37 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index cb5a3b422..94546637e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -556,6 +556,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); + //Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it + m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192); + m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer(); @@ -2694,13 +2697,16 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName) btVector3 gVRGripperPos(0,0,0); btQuaternion gVRGripperOrn(0,0,0,1); - +int gDroppedSimulationSteps = 0; +int gNumSteps = 0; +double gDtInSec = 0.f; +double gSubStep = 0.f; void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) { if (m_data->m_allowRealTimeSimulation && m_data->m_guiHelper) { - btAlignedObjectArray bufferServerToClient; - bufferServerToClient.resize(32768); + static btAlignedObjectArray gBufferServerToClient; + gBufferServerToClient.resize(32768); if (!m_data->m_hasGround) @@ -2710,14 +2716,14 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) int bodyId = 0; - loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size()); + loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); } if (0)//m_data->m_gripperRigidbodyFixed==0) { int bodyId = 0; - loadUrdf("pr2_gripper.urdf",btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &bufferServerToClient[0], bufferServerToClient.size()); + loadUrdf("pr2_gripper.urdf",btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); InteralBodyData* parentBody = m_data->getHandle(bodyId); if (parentBody->m_multiBody) @@ -2747,13 +2753,13 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) int maxSteps = 3; int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps,m_data->m_physicsDeltaTime); - int droppedSteps = numSteps > maxSteps ? numSteps - maxSteps : 0; - static int skipReport = 0; - skipReport++; - if (skipReport>100) + gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0; + + if (numSteps) { - skipReport = 0; - //printf("numSteps: %d (dropped %d), %f (internal step = %f)\n", numSteps, droppedSteps , dtInSec, m_data->m_physicsDeltaTime); + gNumSteps = numSteps; + gDtInSec = dtInSec; + gSubStep = m_data->m_physicsDeltaTime; } } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index ad5fa0fd3..49f63a9a4 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -13,7 +13,8 @@ #include "../Utils/b3Clock.h" #include "../MultiThreading/b3ThreadSupportInterface.h" -#define MAX_VR_CONTROLLERS 4 +#define MAX_VR_CONTROLLERS 8 + extern btVector3 gLastPickPos; btVector3 gVRTeleportPos(0,0,0); @@ -880,6 +881,12 @@ static float vrOffset[16]={1,0,0,0, 0,0,0,0}; +extern int gDroppedSimulationSteps; +extern int gNumSteps; +extern double gDtInSec; +extern double gSubStep; + + void PhysicsServerExample::renderScene() { ///debug rendering @@ -925,24 +932,38 @@ void PhysicsServerExample::renderScene() //some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift) static int frameCount=0; - frameCount++; - char bla[1024]; - 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; + frameCount++; + static char line0[1024]; + static char line1[1024]; + + static btScalar worseFps = 1000000; + int numFrames = 200; + static int count = 0; + count++; + + if (0 == (count & 1)) + { + btScalar curTime = m_clock.getTimeSeconds(); + btScalar fps = 1. / (curTime - prevTime); + prevTime = curTime; + if (fps < worseFps) + { + worseFps = fps; + } + + if (count > numFrames) + { + count = 0; + sprintf(line0, "Graphics FPS (worse) = %f, frame %d", worseFps, frameCount / 2); + + sprintf(line1, "Physics Steps = %d, Dropped = %d, dt %f, Substep %f)", gNumSteps, gDroppedSimulationSteps, gDtInSec, gSubStep); + gDroppedSimulationSteps = 0; + + worseFps = 1000000; + } + } - 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); pos[0]+=gVRTeleportPos[0]; @@ -965,12 +986,12 @@ void PhysicsServerExample::renderScene() 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); + up+=0.6*side; + m_guiHelper->getAppInterface()->drawText3D(line0,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_guiHelper->getAppInterface()->drawText3D(line1,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1); } //m_args[0].m_cs->unlock(); @@ -1083,7 +1104,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt { //printf("controllerId %d, button=%d\n",controllerId, button); - if (controllerId<0 || controllerId>MAX_VR_CONTROLLERS) + if (controllerId<0 || controllerId>=MAX_VR_CONTROLLERS) return; if (button==1 && state==0) @@ -1118,6 +1139,11 @@ extern btQuaternion gVRGripperOrn; void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4]) { + if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS) + { + printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS); + return; + } m_args[0].m_vrControllerPos[controllerId].setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]); m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0],orn[1],orn[2],orn[3]); diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index a9183b43c..aaa453fce 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -625,6 +625,9 @@ void CMainApplication::Shutdown() } } + sExample->exitPhysics(); + delete sExample; + delete m_app; m_app=0; @@ -752,13 +755,11 @@ bool CMainApplication::HandleInput() //----------------------------------------------------------------------------- // Purpose: //----------------------------------------------------------------------------- + void CMainApplication::RunMainLoop() { bool bQuit = false; - //SDL_StartTextInput(); - //SDL_ShowCursor( SDL_DISABLE ); - while ( !bQuit && !m_app->m_window->requestedExit()) { bQuit = HandleInput(); @@ -766,7 +767,6 @@ void CMainApplication::RunMainLoop() RenderFrame(); } - //SDL_StopTextInput(); }