From 4c06fd27b371144d2101474d474c17814f273d3a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 23 Dec 2016 15:20:04 -0800 Subject: [PATCH] Example Browser: add option (keypress 'p') to dump json timing profile trace, that you can open using Chrome about://tracing Make btQuickprof thread safe Add option in btQuickprof to override custom timing profile (btSetCustomEnterProfileZoneFunc, btSetCustomLeaveProfileZoneFunc) remove b3Printf in a user/physics thread (those added added, while drawing the GUI running in the main thread) --- .../GwenGUISupport/GwenProfileWindow.cpp | 12 +- .../GwenGUISupport/GwenProfileWindow.h | 2 + .../ExampleBrowser/OpenGLExampleBrowser.cpp | 233 ++++++++++++++-- .../PhysicsServerCommandProcessor.cpp | 82 ++++-- .../SharedMemory/PhysicsServerExample.cpp | 8 +- examples/Utils/b3Clock.cpp | 60 +--- .../btCollisionDispatcher.cpp | 3 - .../CollisionDispatch/btCollisionWorld.cpp | 2 +- src/LinearMath/btQuickprof.cpp | 256 ++++++++++++------ src/LinearMath/btQuickprof.h | 59 ++-- src/LinearMath/btThreads.cpp | 20 +- src/LinearMath/btThreads.h | 3 +- 12 files changed, 515 insertions(+), 225 deletions(-) diff --git a/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp b/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp index 4991ee04e..dc449c987 100644 --- a/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp +++ b/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp @@ -44,7 +44,7 @@ public: CProfileIterator* profIter; - + class MyMenuItems* m_menuItems; MyProfileWindow ( Gwen::Controls::Base* pParent) : Gwen::Controls::WindowControl( pParent ), @@ -192,7 +192,8 @@ public: // Gwen::Controls::TreeNode* curParent = m_node; - + + double accumulated_time = dumpRecursive(profileIterator,m_node); const char* name = profileIterator->Get_Current_Parent_Name(); @@ -278,7 +279,7 @@ MyProfileWindow* setupProfileWindow(GwenInternalData* data) //profWindow->SetHidden(true); profWindow->m_menuItems = menuItems; - //profWindow->profIter = CProfileManager::Get_Iterator(); + profWindow->profIter = CProfileManager::Get_Iterator(); data->m_viewMenu->GetMenu()->AddItem( L"Profiler", menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::MenuItemSelect); menuItems->m_profWindow = profWindow; @@ -296,6 +297,11 @@ void processProfileData( MyProfileWindow* profWindow, bool idle) } } +bool isProfileWindowVisible(MyProfileWindow* window) +{ + return !window->Hidden(); +} + void profileWindowSetVisible(MyProfileWindow* window, bool visible) { window->SetHidden(!visible); diff --git a/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.h b/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.h index 16058e6f5..fc0ba2982 100644 --- a/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.h +++ b/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.h @@ -4,6 +4,8 @@ class MyProfileWindow* setupProfileWindow(struct GwenInternalData* data); void processProfileData(MyProfileWindow* window, bool idle); void profileWindowSetVisible(MyProfileWindow* window, bool visible); +bool isProfileWindowVisible(MyProfileWindow* window); + void destroyProfileWindow(MyProfileWindow* window); #endif//GWEN_PROFILE_WINDOW_H diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index ca86cda97..768e8c781 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -17,7 +17,7 @@ #endif //_WIN32 #endif//__APPLE__ #include "../ThirdPartyLibs/Gwen/Renderers/OpenGL_DebugFont.h" - +#include "LinearMath/btThreads.h" #include "Bullet3Common/b3Vector3.h" #include "assert.h" #include @@ -25,7 +25,9 @@ #include "GwenGUISupport/gwenUserInterface.h" #include "../Utils/b3Clock.h" #include "GwenGUISupport/GwenParameterInterface.h" +#ifndef BT_NO_PROFILE #include "GwenGUISupport/GwenProfileWindow.h" +#endif #include "GwenGUISupport/GwenTextureWindow.h" #include "GwenGUISupport/GraphingTexture.h" #include "../CommonInterfaces/Common2dCanvasInterface.h" @@ -68,7 +70,9 @@ struct OpenGLExampleBrowserInternalData { Gwen::Renderer::Base* m_gwenRenderer; CommonGraphicsApp* m_app; -// MyProfileWindow* m_profWindow; +#ifndef BT_NO_PROFILE + MyProfileWindow* m_profWindow; +#endif //BT_NO_PROFILE btAlignedObjectArray m_nodes; GwenUserInterface* m_gui; GL3TexLoader* m_myTexLoader; @@ -93,7 +97,9 @@ static CommonWindowInterface* s_window = 0; static CommonParameterInterface* s_parameterInterface=0; static CommonRenderInterface* s_instancingRenderer=0; static OpenGLGuiHelper* s_guiHelper=0; -//static MyProfileWindow* s_profWindow =0; +#ifndef BT_NO_PROFILE +static MyProfileWindow* s_profWindow =0; +#endif //BT_NO_PROFILE static SharedMemoryInterface* sSharedMem = 0; #define DEMO_SELECTION_COMBOBOX 13 @@ -144,6 +150,140 @@ int gGpuArraySizeZ=45; +struct btTiming +{ + const char* m_name; + int m_threadId; + unsigned long long int m_usStartTime; + unsigned long long int m_usEndTime; +}; + +FILE* gTimingFile = 0; +#include +#define BT_TIMING_CAPACITY 65536 +static bool m_firstTiming = true; + + +struct btTimings +{ + btTimings() + :m_numTimings(0), + m_activeBuffer(0) + { + + } + void flush() + { + for (int i=0;i=BT_TIMING_CAPACITY) + { + return; + } + + + + int slot = m_numTimings++; + + m_timings[m_activeBuffer][slot].m_name = name; + m_timings[m_activeBuffer][slot].m_threadId = threadId; + m_timings[m_activeBuffer][slot].m_usStartTime = startTime; + m_timings[m_activeBuffer][slot].m_usEndTime = endTime; + } + + + int m_numTimings; + int m_activeBuffer; + btTiming m_timings[2][BT_TIMING_CAPACITY]; +}; + +btTimings gTimings[BT_MAX_THREAD_COUNT]; + +btClock clk; + +#define MAX_NESTING 1024 + +bool gProfileDisabled = true; +int gStackDepths[BT_MAX_THREAD_COUNT] = {0}; +const char* gFuncNames[BT_MAX_THREAD_COUNT][MAX_NESTING]; +unsigned long long int gStartTimes[BT_MAX_THREAD_COUNT][MAX_NESTING]; + +void MyDummyEnterProfileZoneFunc(const char* msg) +{ +} + +void MyDummyLeaveProfileZoneFunc() +{ +} + +void MyEnterProfileZoneFunc(const char* msg) +{ + if (gProfileDisabled) + return; + int threadId = btGetCurrentThreadIndex(); + + if (gStackDepths[threadId]>=MAX_NESTING) + { + btAssert(0); + return; + } + gFuncNames[threadId][gStackDepths[threadId]] = msg; + gStartTimes[threadId][gStackDepths[threadId]] = clk.getTimeNanoseconds(); + if (gStartTimes[threadId][gStackDepths[threadId]]<=gStartTimes[threadId][gStackDepths[threadId]-1]) + { + gStartTimes[threadId][gStackDepths[threadId]]=1+gStartTimes[threadId][gStackDepths[threadId]-1]; + } + gStackDepths[threadId]++; +} +void MyLeaveProfileZoneFunc() +{ + if (gProfileDisabled) + return; + + int threadId = btGetCurrentThreadIndex(); + + if (gStackDepths[threadId]<=0) + { + return; + } + + gStackDepths[threadId]--; + + const char* name = gFuncNames[threadId][gStackDepths[threadId]]; + unsigned long long int startTime = gStartTimes[threadId][gStackDepths[threadId]]; + + unsigned long long int endTime = clk.getTimeNanoseconds(); + gTimings[threadId].addTiming(name,threadId,startTime,endTime); +} + + void deleteDemo() { if (sCurrentDemo) @@ -154,6 +294,7 @@ void deleteDemo() sCurrentDemo=0; delete s_guiHelper; s_guiHelper = 0; +// CProfileManager::CleanupMemory(); } } @@ -237,6 +378,46 @@ void MyKeyboardCallback(int key, int state) singleStepSimulation = true; } + if (key=='p') + { + if (state) + { + m_firstTiming = true; + gProfileDisabled = false;//true; + b3SetCustomEnterProfileZoneFunc(MyEnterProfileZoneFunc); + b3SetCustomLeaveProfileZoneFunc(MyLeaveProfileZoneFunc); + + //also for Bullet 2.x API + btSetCustomEnterProfileZoneFunc(MyEnterProfileZoneFunc); + btSetCustomLeaveProfileZoneFunc(MyLeaveProfileZoneFunc); + } else + { + + b3SetCustomEnterProfileZoneFunc(MyDummyEnterProfileZoneFunc); + b3SetCustomLeaveProfileZoneFunc(MyDummyLeaveProfileZoneFunc); + //also for Bullet 2.x API + btSetCustomEnterProfileZoneFunc(MyDummyEnterProfileZoneFunc); + btSetCustomLeaveProfileZoneFunc(MyDummyLeaveProfileZoneFunc); + char fileName[1024]; + static int fileCounter = 0; + sprintf(fileName,"d:/timings_%d.json",fileCounter++); + gTimingFile = fopen(fileName,"w"); + fprintf(gTimingFile,"{\"traceEvents\":[\n"); + //dump the content to file + for (int i=0;iexitPhysics(); - s_instancingRenderer->removeAllInstances(); - delete sCurrentDemo; - sCurrentDemo=0; - delete s_guiHelper; - s_guiHelper = 0; - } + deleteDemo(); s_guiHelper= new OpenGLGuiHelper(s_app, sUseOpenGL2); s_parameterInterface->removeAllParameters(); @@ -387,6 +560,7 @@ void selectDemo(int demoIndex) demoIndex = 0; } deleteDemo(); + CommonExampleInterface::CreateFunc* func = gAllExamples->getExampleCreateFunc(demoIndex); if (func) @@ -775,6 +949,8 @@ OpenGLExampleBrowser::~OpenGLExampleBrowser() gAllExamples = 0; } + + #include "EmptyExample.h" bool OpenGLExampleBrowser::init(int argc, char* argv[]) @@ -886,6 +1062,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) b3SetCustomPrintfFunc(MyGuiPrintf); b3SetCustomErrorMessageFunc(MyStatusBarError); + assert(glGetError()==GL_NO_ERROR); @@ -940,10 +1117,11 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) //gui->getInternalData()->pRenderer->setTextureLoader(myTexLoader); - -// s_profWindow= setupProfileWindow(gui2->getInternalData()); - //m_internalData->m_profWindow = s_profWindow; - // profileWindowSetVisible(s_profWindow,false); +#ifndef BT_NO_PROFILE + s_profWindow= setupProfileWindow(gui2->getInternalData()); + m_internalData->m_profWindow = s_profWindow; + profileWindowSetVisible(s_profWindow,false); +#endif //BT_NO_PROFILE gui2->setFocus(); s_parameterInterface = s_app->m_parameterInterface = new GwenParameterInterface(gui2->getInternalData()); @@ -1073,7 +1251,6 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) } - CommonExampleInterface* OpenGLExampleBrowser::getCurrentExample() { btAssert(sCurrentDemo); @@ -1087,6 +1264,8 @@ bool OpenGLExampleBrowser::requestedExit() void OpenGLExampleBrowser::update(float deltaTime) { + gProfileDisabled = false; + B3_PROFILE("OpenGLExampleBrowser::update"); assert(glGetError()==GL_NO_ERROR); s_instancingRenderer->init(); @@ -1136,7 +1315,7 @@ void OpenGLExampleBrowser::update(float deltaTime) { if (!pauseSimulation || singleStepSimulation) { - singleStepSimulation = false; + //printf("---------------------------------------------------\n"); //printf("Framecount = %d\n",frameCount); B3_PROFILE("sCurrentDemo->stepSimulation"); @@ -1167,7 +1346,8 @@ void OpenGLExampleBrowser::update(float deltaTime) } BT_PROFILE("Render Scene"); sCurrentDemo->renderScene(); - } else + } + //else { B3_PROFILE("physicsDebugDraw"); glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); @@ -1198,9 +1378,18 @@ void OpenGLExampleBrowser::update(float deltaTime) if (renderGui) { B3_PROFILE("renderGui"); - // if (!pauseSimulation) - // processProfileData(s_profWindow,false); +#ifndef BT_NO_PROFILE + if (!pauseSimulation || singleStepSimulation) + { + if (isProfileWindowVisible(s_profWindow)) + { + processProfileData(s_profWindow,false); + } + } +#endif //#ifndef BT_NO_PROFILE + + if (sUseOpenGL2) { @@ -1219,7 +1408,7 @@ void OpenGLExampleBrowser::update(float deltaTime) } - + singleStepSimulation = false; toggle=1-toggle; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 5425100ef..1a2091532 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -718,7 +718,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001; m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50; m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7; - +// m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2; + //todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp) + } void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies() @@ -4208,39 +4210,46 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); m_data->m_KukaId = bodyId; - InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId); - if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7) + if (m_data->m_KukaId>=0) { - btScalar q[7]; - q[0] = 0;// -SIMD_HALF_PI; - q[1] = 0; - q[2] = 0; - q[3] = SIMD_HALF_PI; - q[4] = 0; - q[5] = -SIMD_HALF_PI*0.66; - q[6] = 0; - - for (int i = 0; i < 7; i++) + InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId); + if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7) { - kukaBody->m_multiBody->setJointPos(i, q[i]); + btScalar q[7]; + q[0] = 0;// -SIMD_HALF_PI; + q[1] = 0; + q[2] = 0; + q[3] = SIMD_HALF_PI; + q[4] = 0; + q[5] = -SIMD_HALF_PI*0.66; + q[6] = 0; + + for (int i = 0; i < 7; i++) + { + kukaBody->m_multiBody->setJointPos(i, q[i]); + } + btAlignedObjectArray scratch_q; + btAlignedObjectArray scratch_m; + kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m); + int nLinks = kukaBody->m_multiBody->getNumLinks(); + scratch_q.resize(nLinks + 1); + scratch_m.resize(nLinks + 1); + kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m); } - btAlignedObjectArray scratch_q; - btAlignedObjectArray scratch_m; - kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m); - int nLinks = kukaBody->m_multiBody->getNumLinks(); - scratch_q.resize(nLinks + 1); - scratch_m.resize(nLinks + 1); - kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m); } +#if 1 loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("r2d2.urdf", btVector3(-2, -4, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); +#endif + // loadUrdf("r2d2.urdf", btVector3(-2, -4, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + +#if 1 // Load one motor gripper for kuka loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); m_data->m_gripperId = bodyId + 1; - + { InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId); // Reset the default gripper motor maximum torque for damping to 0 @@ -4255,12 +4264,14 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() } } } - + } +#endif +#if 1 for (int i = 0; i < 6; i++) { loadUrdf("jenga/jenga.urdf", btVector3(1.3-0.1*i,-0.7, .75), btQuaternion(btVector3(0,1,0),SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); } - +#endif //loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); // Add slider joint for fingers @@ -4274,6 +4285,10 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() btMatrix3x3 frameInParent2(btQuaternion(0, 0, 0, 1.0)); btMatrix3x3 frameInChild2(btQuaternion(0, 0, 1.0, 0)); btVector3 jointAxis2(1.0, 0, 0); + + if (m_data->m_gripperId>=0) + { + InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId); m_data->m_kukaGripperRevolute1 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 2, gripperBody->m_multiBody, 4, pivotInParent1, pivotInChild1); m_data->m_kukaGripperRevolute1->setMaxAppliedImpulse(5.0); m_data->m_kukaGripperRevolute2 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 3, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2); @@ -4282,9 +4297,17 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1); m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2); - kukaBody = m_data->getHandle(m_data->m_KukaId); + } + + if (m_data->m_KukaId>=0) + { + InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId); if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7) { + if (m_data->m_gripperId>=0) + { + InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId); + gripperBody->m_multiBody->setHasSelfCollision(0); btVector3 pivotInParent(0, 0, 0.05); btMatrix3x3 frameInParent; @@ -4297,16 +4320,20 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() m_data->m_kukaGripperMultiBody = gripperBody->m_multiBody; m_data->m_kukaGripperFixed->setMaxAppliedImpulse(500); m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperFixed); + } } + } +#if 0 for (int i = 0; i < 10; i++) { loadUrdf("cube.urdf", btVector3(-4, -2, 0.5 + i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); } + loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - +#endif btTransform objectLocalTr[] = { btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.0, 0.0)), btTransform(btQuaternion(btVector3(0,0,1),-SIMD_HALF_PI), btVector3(0.0, 0.15, 0.64)), @@ -4340,6 +4367,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() //loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); //loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("cube_small.urdf", objectWorldTr[5].getOrigin(), objectWorldTr[5].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("sphere_small.urdf", objectWorldTr[6].getOrigin(), objectWorldTr[6].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("duck_vhacd.urdf", objectWorldTr[7].getOrigin(), objectWorldTr[7].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 2bc823d3b..1cee22132 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -111,7 +111,7 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2)); gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ); saveCurrentSettingsVR(); - b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ); +// b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ); } if (message->at(1) == 32) @@ -125,7 +125,7 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void { gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2)); saveCurrentSettingsVR(); - b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPos1[i]); +// b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPos1[i]); } } @@ -344,7 +344,7 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) if (deltaTimeInSeconds>clampedDeltaTime) { deltaTimeInSeconds = clampedDeltaTime; - b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime); + //b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime); } clock.reset(); @@ -1060,7 +1060,7 @@ m_options(options) #endif m_multiThreadedHelper = helper; - b3Printf("Started PhysicsServer\n"); +// b3Printf("Started PhysicsServer\n"); } diff --git a/examples/Utils/b3Clock.cpp b/examples/Utils/b3Clock.cpp index 8e1884526..c2db16264 100644 --- a/examples/Utils/b3Clock.cpp +++ b/examples/Utils/b3Clock.cpp @@ -47,7 +47,6 @@ struct b3ClockData #ifdef B3_USE_WINDOWS_TIMERS LARGE_INTEGER mClockFrequency; DWORD mStartTick; - LONGLONG mPrevElapsedTime; LARGE_INTEGER mStartTime; #else #ifdef __CELLOS_LV2__ @@ -94,7 +93,6 @@ void b3Clock::reset() #ifdef B3_USE_WINDOWS_TIMERS QueryPerformanceCounter(&m_data->mStartTime); m_data->mStartTick = GetTickCount(); - m_data->mPrevElapsedTime = 0; #else #ifdef __CELLOS_LV2__ @@ -121,27 +119,7 @@ unsigned long int b3Clock::getTimeMilliseconds() // Compute the number of millisecond ticks elapsed. unsigned long msecTicks = (unsigned 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); - if (msecOff < -100 || msecOff > 100) - { - // Adjust the starting time forwards. - LONGLONG msecAdjustment = b3ClockMin(msecOff * - m_data->mClockFrequency.QuadPart / 1000, elapsedTime - - m_data->mPrevElapsedTime); - m_data->mStartTime.QuadPart += msecAdjustment; - elapsedTime -= msecAdjustment; - - // Recompute the number of millisecond ticks elapsed. - msecTicks = (unsigned long)(1000 * elapsedTime / - m_data->mClockFrequency.QuadPart); - } - - // Store the current elapsed time for adjustments next time. - m_data->mPrevElapsedTime = elapsedTime; + return msecTicks; #else @@ -170,38 +148,16 @@ unsigned long int b3Clock::getTimeMilliseconds() unsigned long long int b3Clock::getTimeMicroseconds() { #ifdef B3_USE_WINDOWS_TIMERS - LARGE_INTEGER currentTime; + //see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx + LARGE_INTEGER currentTime, elapsedTime; + QueryPerformanceCounter(¤tTime); - LONGLONG elapsedTime = currentTime.QuadPart - + elapsedTime.QuadPart = currentTime.QuadPart - m_data->mStartTime.QuadPart; + elapsedTime.QuadPart *= 1000000; + elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart; - // Compute the number of millisecond ticks elapsed. - 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 long elapsedTicks = GetTickCount() - m_data->mStartTick; - signed long long msecOff = (signed long)(msecTicks - elapsedTicks); - if (msecOff < -100 || msecOff > 100) - { - // Adjust the starting time forwards. - LONGLONG msecAdjustment = b3ClockMin(msecOff * - m_data->mClockFrequency.QuadPart / 1000, elapsedTime - - m_data->mPrevElapsedTime); - m_data->mStartTime.QuadPart += msecAdjustment; - elapsedTime -= msecAdjustment; - } - - // Store the current elapsed time for adjustments next time. - m_data->mPrevElapsedTime = elapsedTime; - - // Convert to microseconds. - unsigned long long usecTicks = (unsigned long)(1000000 * elapsedTime / - m_data->mClockFrequency.QuadPart); - - return usecTicks; + return (unsigned long long) elapsedTime.QuadPart; #else #ifdef __CELLOS_LV2__ diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 737067ef9..5739a1ef0 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -239,10 +239,7 @@ public: virtual bool processOverlap(btBroadphasePair& pair) { - BT_PROFILE("btCollisionDispatcher::processOverlap"); - (*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo); - return false; } }; diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 3bbf7586e..1b7d7e39d 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -257,7 +257,7 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) int iObj = collisionObject->getWorldArrayIndex(); - btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously? +// btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously? if (iObj >= 0 && iObj < m_collisionObjects.size()) { btAssert(collisionObject == m_collisionObjects[iObj]); diff --git a/src/LinearMath/btQuickprof.cpp b/src/LinearMath/btQuickprof.cpp index f587770e8..63e855db8 100644 --- a/src/LinearMath/btQuickprof.cpp +++ b/src/LinearMath/btQuickprof.cpp @@ -16,9 +16,7 @@ #include "btQuickprof.h" -#if BT_THREADSAFE -#include "btThreads.h" -#endif //#if BT_THREADSAFE + #ifdef __CELLOS_LV2__ @@ -65,7 +63,6 @@ struct btClockData #ifdef BT_USE_WINDOWS_TIMERS LARGE_INTEGER mClockFrequency; LONGLONG mStartTick; - LONGLONG mPrevElapsedTime; LARGE_INTEGER mStartTime; #else #ifdef __CELLOS_LV2__ @@ -111,7 +108,6 @@ void btClock::reset() #ifdef BT_USE_WINDOWS_TIMERS QueryPerformanceCounter(&m_data->mStartTime); m_data->mStartTick = GetTickCount64(); - m_data->mPrevElapsedTime = 0; #else #ifdef __CELLOS_LV2__ @@ -128,7 +124,7 @@ void btClock::reset() /// Returns the time in ms since the last call to reset or since /// the btClock was created. -unsigned long int btClock::getTimeMilliseconds() +unsigned long long int btClock::getTimeMilliseconds() { #ifdef BT_USE_WINDOWS_TIMERS LARGE_INTEGER currentTime; @@ -138,27 +134,6 @@ unsigned long int btClock::getTimeMilliseconds() // Compute the number of millisecond ticks elapsed. unsigned long msecTicks = (unsigned 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 = (unsigned long)(GetTickCount64() - m_data->mStartTick); - signed long msecOff = (signed long)(msecTicks - elapsedTicks); - if (msecOff < -100 || msecOff > 100) - { - // Adjust the starting time forwards. - LONGLONG msecAdjustment = mymin(msecOff * - m_data->mClockFrequency.QuadPart / 1000, elapsedTime - - m_data->mPrevElapsedTime); - m_data->mStartTime.QuadPart += msecAdjustment; - elapsedTime -= msecAdjustment; - - // Recompute the number of millisecond ticks elapsed. - msecTicks = (unsigned long)(1000 * elapsedTime / - m_data->mClockFrequency.QuadPart); - } - - // Store the current elapsed time for adjustments next time. - m_data->mPrevElapsedTime = elapsedTime; return msecTicks; #else @@ -184,41 +159,19 @@ unsigned long int btClock::getTimeMilliseconds() /// Returns the time in us since the last call to reset or since /// the Clock was created. -unsigned long int btClock::getTimeMicroseconds() +unsigned long long int btClock::getTimeMicroseconds() { #ifdef BT_USE_WINDOWS_TIMERS - LARGE_INTEGER currentTime; + //see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx + LARGE_INTEGER currentTime, elapsedTime; + QueryPerformanceCounter(¤tTime); - LONGLONG elapsedTime = currentTime.QuadPart - + elapsedTime.QuadPart = currentTime.QuadPart - m_data->mStartTime.QuadPart; + elapsedTime.QuadPart *= 1000000; + elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart; - // Compute the number of millisecond ticks elapsed. - unsigned long msecTicks = (unsigned 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 = (unsigned long)(GetTickCount64() - m_data->mStartTick); - signed long msecOff = (signed long)(msecTicks - elapsedTicks); - if (msecOff < -100 || msecOff > 100) - { - // Adjust the starting time forwards. - LONGLONG msecAdjustment = mymin(msecOff * - m_data->mClockFrequency.QuadPart / 1000, elapsedTime - - m_data->mPrevElapsedTime); - m_data->mStartTime.QuadPart += msecAdjustment; - elapsedTime -= msecAdjustment; - } - - // Store the current elapsed time for adjustments next time. - m_data->mPrevElapsedTime = elapsedTime; - - // Convert to microseconds. - unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / - m_data->mClockFrequency.QuadPart); - - return usecTicks; + return (unsigned long long) elapsedTime.QuadPart; #else #ifdef __CELLOS_LV2__ @@ -240,6 +193,39 @@ unsigned long int btClock::getTimeMicroseconds() #endif } +unsigned long long int btClock::getTimeNanoseconds() +{ +#ifdef BT_USE_WINDOWS_TIMERS + //see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx + LARGE_INTEGER currentTime, elapsedTime; + + QueryPerformanceCounter(¤tTime); + elapsedTime.QuadPart = currentTime.QuadPart - + m_data->mStartTime.QuadPart; + elapsedTime.QuadPart *= 1e9; + elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart; + + return (unsigned long long) elapsedTime.QuadPart; +#else + +#ifdef __CELLOS_LV2__ + uint64_t freq=sys_time_get_timebase_frequency(); + double dFreq=((double) freq)/ 1e9; + typedef uint64_t ClockSize; + ClockSize newTime; + //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); + SYS_TIMEBASE_GET( newTime ); + + return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq); +#else + + struct timeval currentTime; + gettimeofday(¤tTime, 0); + return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1e9 + + (currentTime.tv_usec - m_data->mStartTime.tv_usec); +#endif//__CELLOS_LV2__ +#endif +} /// Returns the time in s since the last call to reset or since @@ -370,6 +356,7 @@ bool CProfileNode::Return( void ) if ( --RecursionCounter == 0 && TotalCalls != 0 ) { unsigned long int time; Profile_Get_Ticks(&time); + time-=StartTime; TotalTime += (float)time / Profile_Get_Tick_Rate(); } @@ -437,11 +424,69 @@ void CProfileIterator::Enter_Parent( void ) ** ***************************************************************************************************/ -CProfileNode CProfileManager::Root( "Root", NULL ); -CProfileNode * CProfileManager::CurrentNode = &CProfileManager::Root; +#include "btThreads.h" + + + +CProfileNode gRoots[BT_MAX_THREAD_COUNT]={ + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL) +}; + + +CProfileNode* gCurrentNodes[BT_MAX_THREAD_COUNT]= +{ + &gRoots[ 0], &gRoots[ 1], &gRoots[ 2], &gRoots[ 3], + &gRoots[ 4], &gRoots[ 5], &gRoots[ 6], &gRoots[ 7], + &gRoots[ 8], &gRoots[ 9], &gRoots[10], &gRoots[11], + &gRoots[12], &gRoots[13], &gRoots[14], &gRoots[15], + &gRoots[16], &gRoots[17], &gRoots[18], &gRoots[19], + &gRoots[20], &gRoots[21], &gRoots[22], &gRoots[23], + &gRoots[24], &gRoots[25], &gRoots[26], &gRoots[27], + &gRoots[28], &gRoots[29], &gRoots[30], &gRoots[31], + &gRoots[32], &gRoots[33], &gRoots[34], &gRoots[35], + &gRoots[36], &gRoots[37], &gRoots[38], &gRoots[39], + &gRoots[40], &gRoots[41], &gRoots[42], &gRoots[43], + &gRoots[44], &gRoots[45], &gRoots[46], &gRoots[47], + &gRoots[48], &gRoots[49], &gRoots[50], &gRoots[51], + &gRoots[52], &gRoots[53], &gRoots[54], &gRoots[55], + &gRoots[56], &gRoots[57], &gRoots[58], &gRoots[59], + &gRoots[60], &gRoots[61], &gRoots[62], &gRoots[63], +}; + + int CProfileManager::FrameCounter = 0; unsigned long int CProfileManager::ResetTime = 0; +CProfileIterator * CProfileManager::Get_Iterator( void ) +{ + + int threadIndex = btGetCurrentThreadIndex(); + return new CProfileIterator( &gRoots[threadIndex]); +} + +void CProfileManager::CleanupMemory(void) +{ + for (int i=0;iGet_Name()) { - CurrentNode = CurrentNode->Get_Sub_Node( name ); + int threadIndex = btGetCurrentThreadIndex(); + if (name != gCurrentNodes[threadIndex]->Get_Name()) { + gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Sub_Node( name ); } - CurrentNode->Call(); + gCurrentNodes[threadIndex]->Call(); } @@ -479,22 +517,26 @@ void CProfileManager::Start_Profile( const char * name ) *=============================================================================================*/ void CProfileManager::Stop_Profile( void ) { -#if BT_THREADSAFE - // profile system is not designed for profiling multiple threads - // disable collection on all but the main thread - if ( !btIsMainThread() ) - { - return; - } -#endif //#if BT_THREADSAFE + int threadIndex = btGetCurrentThreadIndex(); // Return will indicate whether we should back up to our parent (we may // be profiling a recursive function) - if (CurrentNode->Return()) { - CurrentNode = CurrentNode->Get_Parent(); + if (gCurrentNodes[threadIndex]->Return()) { + gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Parent(); } } +void btEnterProfileZoneDefault(const char* name) +{ + CProfileManager::Start_Profile( name ); +} +void btLeaveProfileZoneDefault() +{ + CProfileManager::Stop_Profile(); +} + + + /*********************************************************************************************** * CProfileManager::Reset -- Reset the contents of the profiling system * * * @@ -503,8 +545,8 @@ void CProfileManager::Stop_Profile( void ) void CProfileManager::Reset( void ) { gProfileClock.reset(); - Root.Reset(); - Root.Call(); + gRoots[btGetCurrentThreadIndex()].Reset(); + gRoots[btGetCurrentThreadIndex()].Call(); FrameCounter = 0; Profile_Get_Ticks(&ResetTime); } @@ -592,6 +634,56 @@ void CProfileManager::dumpAll() } +#else +void btEnterProfileZoneDefault(const char* name) +{ +} +void btLeaveProfileZoneDefault() +{ +} #endif //BT_NO_PROFILE + + +static btEnterProfileZoneFunc* bts_enterFunc = btEnterProfileZoneDefault; +static btLeaveProfileZoneFunc* bts_leaveFunc = btLeaveProfileZoneDefault; + +void btEnterProfileZone(const char* name) +{ + (bts_enterFunc)(name); +} +void btLeaveProfileZone() +{ + (bts_leaveFunc)(); +} + +btEnterProfileZoneFunc* btGetCurrentEnterProfileZoneFunc() +{ + return bts_enterFunc ; +} +btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc() +{ + return bts_leaveFunc; +} + + +void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc) +{ + bts_enterFunc = enterFunc; +} +void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc) +{ + bts_leaveFunc = leaveFunc; +} + +CProfileSample::CProfileSample( const char * name ) +{ + btEnterProfileZone(name); +} + +CProfileSample::~CProfileSample( void ) +{ + btLeaveProfileZone(); +} + diff --git a/src/LinearMath/btQuickprof.h b/src/LinearMath/btQuickprof.h index 49545713b..664aa4d45 100644 --- a/src/LinearMath/btQuickprof.h +++ b/src/LinearMath/btQuickprof.h @@ -36,12 +36,14 @@ public: /// Returns the time in ms since the last call to reset or since /// the btClock was created. - unsigned long int getTimeMilliseconds(); + unsigned long long int getTimeMilliseconds(); /// 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(); + unsigned long long int getTimeNanoseconds(); + /// Returns the time in s since the last call to reset or since /// the Clock was created. btScalar getTimeSeconds(); @@ -52,9 +54,18 @@ private: #endif //USE_BT_CLOCK +typedef void (btEnterProfileZoneFunc)(const char* msg); +typedef void (btLeaveProfileZoneFunc)(); + +btEnterProfileZoneFunc* btGetCurrentEnterProfileZoneFunc(); +btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc(); + +void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc); +void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc); + //To disable built-in profiling, please comment out next line -#define BT_NO_PROFILE 1 +//#define BT_NO_PROFILE 1 #ifndef BT_NO_PROFILE #include //@todo remove this, backwards compatibility @@ -151,21 +162,21 @@ public: static void Start_Profile( const char * name ); static void Stop_Profile( void ); - static void CleanupMemory(void) - { - Root.CleanupMemory(); - } + static void CleanupMemory(void); +// { +// Root.CleanupMemory(); +// } static void Reset( void ); static void Increment_Frame_Counter( void ); static int Get_Frame_Count_Since_Reset( void ) { return FrameCounter; } static float Get_Time_Since_Reset( void ); - static CProfileIterator * Get_Iterator( void ) - { - - return new CProfileIterator( &Root ); - } + static CProfileIterator * Get_Iterator( void ); +// { +// +// return new CProfileIterator( &Root ); +// } static void Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); } static void dumpRecursive(CProfileIterator* profileIterator, int spacing); @@ -173,37 +184,27 @@ public: static void dumpAll(); private: - static CProfileNode Root; - static CProfileNode * CurrentNode; + static int FrameCounter; static unsigned long int ResetTime; }; + + +#endif //#ifndef BT_NO_PROFILE + ///ProfileSampleClass is a simple way to profile a function's scope ///Use the BT_PROFILE macro at the start of scope to time class CProfileSample { public: - CProfileSample( const char * name ) - { - CProfileManager::Start_Profile( name ); - } + CProfileSample( const char * name ); - ~CProfileSample( void ) - { - CProfileManager::Stop_Profile(); - } + ~CProfileSample( void ); }; - #define BT_PROFILE( name ) CProfileSample __profile( name ) -#else - -#define BT_PROFILE( name ) - -#endif //#ifndef BT_NO_PROFILE - #endif //BT_QUICK_PROF_H diff --git a/src/LinearMath/btThreads.cpp b/src/LinearMath/btThreads.cpp index 4bef499f7..f3501eec9 100644 --- a/src/LinearMath/btThreads.cpp +++ b/src/LinearMath/btThreads.cpp @@ -226,5 +226,23 @@ bool btSpinMutex::tryLock() return true; } -#endif // #else // #if BT_THREADSAFE +unsigned int btGetCurrentThreadIndex() +{ + const unsigned int kNullIndex = ~0U; +#ifdef _WIN32 + __declspec( thread ) static unsigned int sThreadIndex = kNullIndex; + +#else + static __thread unsigned int sThreadIndex = kNullIndex; +#endif + static int gThreadCounter=0; + + if ( sThreadIndex == kNullIndex ) + { + sThreadIndex = gThreadCounter++; + } + return sThreadIndex; +} + +#endif // #if BT_THREADSAFE diff --git a/src/LinearMath/btThreads.h b/src/LinearMath/btThreads.h index db710979f..7a29ab4c0 100644 --- a/src/LinearMath/btThreads.h +++ b/src/LinearMath/btThreads.h @@ -69,7 +69,8 @@ const unsigned int BT_MAX_THREAD_COUNT = 64; SIMD_FORCE_INLINE void btMutexLock( btSpinMutex* ) {} SIMD_FORCE_INLINE void btMutexUnlock( btSpinMutex* ) {} SIMD_FORCE_INLINE bool btMutexTryLock( btSpinMutex* ) {return true;} - +unsigned int btGetCurrentThreadIndex(); +const unsigned int BT_MAX_THREAD_COUNT = 64; #endif