diff --git a/examples/BasicDemo/CMakeLists.txt b/examples/BasicDemo/CMakeLists.txt index f01a03a51..79037e01e 100644 --- a/examples/BasicDemo/CMakeLists.txt +++ b/examples/BasicDemo/CMakeLists.txt @@ -60,7 +60,7 @@ SET(AppBasicExampleGui_SRCS ADD_DEFINITIONS(-DB3_USE_STANDALONE_EXAMPLE) LINK_LIBRARIES( - BulletDynamics BulletCollision LinearMath OpenGLWindow Bullet3Common ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} + BulletDynamics BulletCollision LinearMath OpenGLWindow Bullet3Common ) #some code to support OpenGL and Glew cross platform @@ -69,6 +69,7 @@ IF (WIN32) ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows ) ADD_DEFINITIONS(-DGLEW_STATIC) + LINK_LIBRARIES( ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} ) ELSE(WIN32) IF(APPLE) find_library(COCOA NAMES Cocoa) @@ -83,7 +84,7 @@ ELSE(WIN32) ADD_DEFINITIONS("-DGLEW_STATIC") ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1") - LINK_LIBRARIES( X11 pthread dl Xext) + LINK_LIBRARIES( pthread dl ) ENDIF(APPLE) ENDIF(WIN32) 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..ff1b9588a 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,198 @@ 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; +#ifndef __STDC_FORMAT_MACROS +#define __STDC_FORMAT_MACROS +#endif //__STDC_FORMAT_MACROS +#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;iendTime) + { + endTime = startTime; + } + unsigned int startTimeRem1000 = startTime%1000; + unsigned int endTimeRem1000 = endTime%1000; + + char startTimeRem1000Str[16]; + char endTimeRem1000Str[16]; + + if (startTimeRem1000<10) + { + sprintf(startTimeRem1000Str,"00%d",startTimeRem1000); + } + else + { + if (startTimeRem1000<100) + { + sprintf(startTimeRem1000Str,"0%d",startTimeRem1000); + } else + { + sprintf(startTimeRem1000Str,"%d",startTimeRem1000); + } + } + + if (endTimeRem1000<10) + { + sprintf(endTimeRem1000Str,"00%d",endTimeRem1000); + } + else + { + if (endTimeRem1000<100) + { + sprintf(endTimeRem1000Str,"0%d",endTimeRem1000); + } else + { + sprintf(endTimeRem1000Str,"%d",endTimeRem1000); + } + } + + char newname[1024]; + static int counter2=0; + + sprintf(newname,"%s%d",name,counter2++); + fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n", + threadId, startTimeDiv1000,startTimeRem1000Str, newname); + fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}", + threadId, endTimeDiv1000,endTimeRem1000Str,newname); +#endif + + } + m_numTimings = 0; + + } + + void addTiming(const char* name, int threadId, unsigned long long int startTime, unsigned long long int endTime) + { + if (m_numTimings>=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 +352,7 @@ void deleteDemo() sCurrentDemo=0; delete s_guiHelper; s_guiHelper = 0; +// CProfileManager::CleanupMemory(); } } @@ -237,6 +436,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,"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 +618,7 @@ void selectDemo(int demoIndex) demoIndex = 0; } deleteDemo(); + CommonExampleInterface::CreateFunc* func = gAllExamples->getExampleCreateFunc(demoIndex); if (func) @@ -775,6 +1007,8 @@ OpenGLExampleBrowser::~OpenGLExampleBrowser() gAllExamples = 0; } + + #include "EmptyExample.h" bool OpenGLExampleBrowser::init(int argc, char* argv[]) @@ -886,6 +1120,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) b3SetCustomPrintfFunc(MyGuiPrintf); b3SetCustomErrorMessageFunc(MyStatusBarError); + assert(glGetError()==GL_NO_ERROR); @@ -940,10 +1175,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 +1309,6 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) } - CommonExampleInterface* OpenGLExampleBrowser::getCurrentExample() { btAssert(sCurrentDemo); @@ -1087,6 +1322,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 +1373,7 @@ void OpenGLExampleBrowser::update(float deltaTime) { if (!pauseSimulation || singleStepSimulation) { - singleStepSimulation = false; + //printf("---------------------------------------------------\n"); //printf("Framecount = %d\n",frameCount); B3_PROFILE("sCurrentDemo->stepSimulation"); @@ -1167,7 +1404,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 +1436,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 +1466,7 @@ void OpenGLExampleBrowser::update(float deltaTime) } - + singleStepSimulation = false; toggle=1-toggle; diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index e1c20cadb..3e3248a0b 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -52,6 +52,10 @@ public: virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) = 0; + virtual void getCachedVREvents(struct b3VREventsData* vrEventsData) = 0; + + virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits) = 0; + }; #endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index a9d217c3e..89c180374 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1122,6 +1122,38 @@ b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle phys return (b3SharedMemoryCommandHandle)command; } +b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX, + double rayFromWorldY, double rayFromWorldZ, + double rayToWorldX, double rayToWorldY, double rayToWorldZ) +{ + PhysicsClient *cl = (PhysicsClient *)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS; + command->m_requestRaycastIntersections.m_rayFromPosition[0] = rayFromWorldX; + command->m_requestRaycastIntersections.m_rayFromPosition[1] = rayFromWorldY; + command->m_requestRaycastIntersections.m_rayFromPosition[2] = rayFromWorldZ; + command->m_requestRaycastIntersections.m_rayToPosition[0] = rayToWorldX; + command->m_requestRaycastIntersections.m_rayToPosition[1] = rayToWorldY; + command->m_requestRaycastIntersections.m_rayToPosition[2] = rayToWorldZ; + + return (b3SharedMemoryCommandHandle)command; + +} + +void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + if (cl) + { + cl->getCachedRaycastHits(raycastInfo); + } +} + + + b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -2135,3 +2167,26 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status return true; } + +b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_REQUEST_VR_EVENTS_DATA; + command->m_updateFlags = 0; + + return (b3SharedMemoryCommandHandle)command; +} + +void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + if (cl) + { + cl->getCachedVREvents(vrEventsData); + } +} diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 97b6a14a6..b8b27dd53 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -291,6 +291,14 @@ b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, d double rayToWorldZ); b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient); +b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX, + double rayFromWorldY, double rayFromWorldZ, + double rayToWorldX, double rayToWorldY, double rayToWorldZ); + +void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo); + + + /// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates. b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient); void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags); @@ -302,6 +310,12 @@ int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale) int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass); int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin); + +b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient); +void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData); + + + #ifdef __cplusplus } #endif diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index e02d11e2d..ae0ab604b 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -42,6 +42,8 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_cachedContactPoints; btAlignedObjectArray m_cachedOverlappingObjects; btAlignedObjectArray m_cachedVisualShapes; + btAlignedObjectArray m_cachedVREvents; + btAlignedObjectArray m_raycastHits; btAlignedObjectArray m_bodyIdsRequestInfo; SharedMemoryStatus m_tempBackupServerStatus; @@ -631,6 +633,35 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Overlapping object query failed"); break; } + + case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Raycast completed"); + } + m_data->m_raycastHits.clear(); + for (int i=0;im_raycastHits.push_back(serverCmd.m_raycastHits.m_rayHits[i]); + } + break; + } + + case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Request VR Events completed"); + } + m_data->m_cachedVREvents.clear(); + for (int i=0;i< serverCmd.m_sendVREvents.m_numVRControllerEvents;i++) + { + m_data->m_cachedVREvents.push_back(serverCmd.m_sendVREvents.m_controllerEvents[i]); + } + break; + } + case CMD_REQUEST_AABB_OVERLAP_COMPLETED: { if (m_data->m_verboseOutput) @@ -983,6 +1014,19 @@ void PhysicsClientSharedMemory::getCachedOverlappingObjects(struct b3AABBOverlap &m_data->m_cachedOverlappingObjects[0] : 0; } +void PhysicsClientSharedMemory::getCachedVREvents(struct b3VREventsData* vrEventsData) +{ + vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size(); + vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents? + &m_data->m_cachedVREvents[0] : 0; +} + +void PhysicsClientSharedMemory::getCachedRaycastHits(struct b3RaycastInformation* raycastHits) +{ + raycastHits->m_numRayHits = m_data->m_raycastHits.size(); + raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0; +} + void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) { @@ -1010,3 +1054,4 @@ const float* PhysicsClientSharedMemory::getDebugLinesColor() const { return 0; } int PhysicsClientSharedMemory::getNumDebugLines() const { return m_data->m_debugLinesFrom.size(); } + diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index d5f6b3cff..854618456 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -60,6 +60,11 @@ public: virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects); virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo); + + virtual void getCachedVREvents(struct b3VREventsData* vrEventsData); + + virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); + }; #endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index cb7a16abd..b12438cc2 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -47,7 +47,10 @@ struct PhysicsDirectInternalData btAlignedObjectArray m_cachedOverlappingObjects; btAlignedObjectArray m_cachedVisualShapes; - + btAlignedObjectArray m_cachedVREvents; + + btAlignedObjectArray m_raycastHits; + PhysicsCommandProcessorInterface* m_commandProcessor; bool m_ownsCommandProcessor; @@ -581,6 +584,34 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd switch (serverCmd.m_type) { + case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Raycast completed"); + } + m_data->m_raycastHits.clear(); + for (int i=0;im_raycastHits.push_back(serverCmd.m_raycastHits.m_rayHits[i]); + } + break; + } + case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED: + { + + if (m_data->m_verboseOutput) + { + b3Printf("Request VR Events completed"); + } + m_data->m_cachedVREvents.clear(); + for (int i=0;i< serverCmd.m_sendVREvents.m_numVRControllerEvents;i++) + { + m_data->m_cachedVREvents.push_back(serverCmd.m_sendVREvents.m_controllerEvents[i]); + } + break; + } + case CMD_REQUEST_INTERNAL_DATA_COMPLETED: { if (serverCmd.m_numDataStreamBytes) @@ -851,3 +882,16 @@ void PhysicsDirect::getCachedVisualShapeInformation(struct b3VisualShapeInformat visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size(); visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0; } + +void PhysicsDirect::getCachedVREvents(struct b3VREventsData* vrEventsData) +{ + vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size(); + vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents? + &m_data->m_cachedVREvents[0] : 0; +} + +void PhysicsDirect::getCachedRaycastHits(struct b3RaycastInformation* raycastHits) +{ + raycastHits->m_numRayHits = m_data->m_raycastHits.size(); + raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0; +} diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 963281f7e..6b6b526be 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -81,6 +81,9 @@ public: virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo); + virtual void getCachedVREvents(struct b3VREventsData* vrEventsData); + + virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); //those 2 APIs are for internal use for visualization virtual bool connect(struct GUIHelperInterface* guiHelper); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 3e9c3d77d..626653f43 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -146,8 +146,17 @@ void PhysicsLoopBack::getCachedVisualShapeInformation(struct b3VisualShapeInform return m_data->m_physicsClient->getCachedVisualShapeInformation(visualShapesInfo); } +void PhysicsLoopBack::getCachedVREvents(struct b3VREventsData* vrEventsData) +{ + return m_data->m_physicsClient->getCachedVREvents(vrEventsData); +} + void PhysicsLoopBack::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects) { return m_data->m_physicsClient->getCachedOverlappingObjects(overlappingObjects); } +void PhysicsLoopBack::getCachedRaycastHits(struct b3RaycastInformation* raycastHits) +{ + return m_data->m_physicsClient->getCachedRaycastHits(raycastHits); +} diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index c9187c4b8..0eaea6ff8 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -65,6 +65,11 @@ public: virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects); virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo); + + virtual void getCachedVREvents(struct b3VREventsData* vrEventsData); + + virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); + }; #endif //PHYSICS_LOOP_BACK_H diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 5425100ef..93bb085f9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -463,6 +463,8 @@ struct PhysicsServerCommandProcessorInternalData bool m_allowRealTimeSimulation; bool m_hasGround; + b3VRControllerEvent m_vrEvents[MAX_VR_CONTROLLERS]; + btMultiBodyFixedConstraint* m_gripperRigidbodyFixed; btMultiBody* m_gripperMultiBody; btMultiBodyFixedConstraint* m_kukaGripperFixed; @@ -562,6 +564,15 @@ struct PhysicsServerCommandProcessorInternalData m_pickedConstraint(0), m_pickingMultiBodyPoint2Point(0) { + for (int i=0;im_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() @@ -1306,6 +1319,85 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } #endif + case CMD_REQUEST_VR_EVENTS_DATA: + { + serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0; + for (int i=0;im_vrEvents[i].m_numButtonEvents + m_data->m_vrEvents[i].m_numMoveEvents) + { + serverStatusOut.m_sendVREvents.m_controllerEvents[serverStatusOut.m_sendVREvents.m_numVRControllerEvents++] = m_data->m_vrEvents[i]; + m_data->m_vrEvents[i].m_numButtonEvents = 0; + m_data->m_vrEvents[i].m_numMoveEvents = 0; + for (int b=0;bm_vrEvents[i].m_buttons[b] = 0; + } + } + } + serverStatusOut.m_type = CMD_REQUEST_VR_EVENTS_DATA_COMPLETED; + hasStatus = true; + break; + }; + case CMD_REQUEST_RAY_CAST_INTERSECTIONS: + { + btVector3 rayFromWorld(clientCmd.m_requestRaycastIntersections.m_rayFromPosition[0], + clientCmd.m_requestRaycastIntersections.m_rayFromPosition[1], + clientCmd.m_requestRaycastIntersections.m_rayFromPosition[2]); + btVector3 rayToWorld(clientCmd.m_requestRaycastIntersections.m_rayToPosition[0], + clientCmd.m_requestRaycastIntersections.m_rayToPosition[1], + clientCmd.m_requestRaycastIntersections.m_rayToPosition[2]); + btCollisionWorld::ClosestRayResultCallback rayResultCallback(rayFromWorld,rayToWorld); + m_data->m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,rayResultCallback); + serverStatusOut.m_raycastHits.m_numRaycastHits = 0; + + if (rayResultCallback.hasHit()) + { + serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitFraction + = rayResultCallback.m_closestHitFraction; + + int objectUniqueId = -1; + int linkIndex = -1; + + const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject); + if (body) + { + objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2(); + } else + { + const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(rayResultCallback.m_collisionObject); + if (mblB && mblB->m_multiBody) + { + linkIndex = mblB->m_link; + objectUniqueId = mblB->m_multiBody->getUserIndex2(); + } + } + + serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectUniqueId + = objectUniqueId; + serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectLinkIndex + = linkIndex; + + serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[0] + = rayResultCallback.m_hitPointWorld[0]; + serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[1] + = rayResultCallback.m_hitPointWorld[1]; + serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[2] + = rayResultCallback.m_hitPointWorld[2]; + + serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[0] + = rayResultCallback.m_hitNormalWorld[0]; + serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[1] + = rayResultCallback.m_hitNormalWorld[1]; + serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[2] + = rayResultCallback.m_hitNormalWorld[2]; + + serverStatusOut.m_raycastHits.m_numRaycastHits++; + } + serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED; + hasStatus = true; + break; + }; case CMD_REQUEST_DEBUG_LINES: { int curFlags =m_data->m_remoteDebugDrawer->getDebugMode(); @@ -4052,8 +4144,46 @@ void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTime m_data->m_allowRealTimeSimulation = enableRealTimeSim; } -void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) +void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents) { + //update m_vrEvents + for (int i=0;im_vrEvents[controlledId].m_analogAxis = vrEvents[i].m_analogAxis; + } + + if (vrEvents[i].m_numMoveEvents+vrEvents[i].m_numButtonEvents) + { + m_data->m_vrEvents[controlledId].m_controllerId = vrEvents[i].m_controllerId; + + m_data->m_vrEvents[controlledId].m_pos[0] = vrEvents[i].m_pos[0]; + m_data->m_vrEvents[controlledId].m_pos[1] = vrEvents[i].m_pos[1]; + m_data->m_vrEvents[controlledId].m_pos[2] = vrEvents[i].m_pos[2]; + + m_data->m_vrEvents[controlledId].m_orn[0] = vrEvents[i].m_orn[0]; + m_data->m_vrEvents[controlledId].m_orn[1] = vrEvents[i].m_orn[1]; + m_data->m_vrEvents[controlledId].m_orn[2] = vrEvents[i].m_orn[2]; + m_data->m_vrEvents[controlledId].m_orn[3] = vrEvents[i].m_orn[3]; + } + + m_data->m_vrEvents[controlledId].m_numButtonEvents += vrEvents[i].m_numButtonEvents; + m_data->m_vrEvents[controlledId].m_numMoveEvents += vrEvents[i].m_numMoveEvents; + for (int b=0;bm_vrEvents[controlledId].m_buttons[b] |= vrEvents[i].m_buttons[b]; + if (vrEvents[i].m_buttons[b] & eButtonIsDown) + { + m_data->m_vrEvents[controlledId].m_buttons[b] |= eButtonIsDown; + } else + { + m_data->m_vrEvents[controlledId].m_buttons[b] &= ~eButtonIsDown; + } + } + } + if (gResetSimulation) { resetSimulation(); @@ -4208,39 +4338,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 +4392,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 +4413,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 +4425,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 +4448,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 +4495,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/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index c8a060069..c4fd8e7a3 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -84,7 +84,7 @@ public: void enableCommandLogging(bool enable, const char* fileName); void replayFromLogFile(const char* fileName); void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes ); - void stepSimulationRealTime(double dtInSec); + void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents); void enableRealTimeSimulation(bool enableRealTimeSim); void applyJointDamping(int bodyUniqueId); }; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 2bc823d3b..d176e5d07 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -14,6 +14,7 @@ #include "Bullet3Common/b3Matrix3x3.h" #include "../Utils/b3Clock.h" #include "../MultiThreading/b3ThreadSupportInterface.h" +#include "SharedMemoryPublic.h" #ifdef BT_ENABLE_VR #include "../RenderingExamples/TinyVRGui.h" #endif//BT_ENABLE_VR @@ -21,7 +22,6 @@ #include "../CommonInterfaces/CommonParameterInterface.h" -#define MAX_VR_CONTROLLERS 8 //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet! @@ -44,7 +44,7 @@ extern bool gResetSimulation; extern int gEnableKukaControl; int gGraspingController = -1; extern btScalar simTimeScalingFactor; - +bool gBatchUserDebugLines = true; extern bool gVRGripperClosed; const char* startFileNameVR = "0_VRDemoSettings.txt"; @@ -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]); } } @@ -225,6 +225,13 @@ struct MotionArgs { for (int i=0;i m_mouseCommands; + + b3VRControllerEvent m_vrControllerEvents[MAX_VR_CONTROLLERS]; + b3VRControllerEvent m_sendVrControllerEvents[MAX_VR_CONTROLLERS]; + PhysicsServerSharedMemory* m_physicsServerPtr; b3AlignedObjectArray m_positions; @@ -344,11 +355,37 @@ 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(); - args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds); + + args->m_cs->lock(); + + int numSendVrControllers = 0; + for (int i=0;im_vrControllerEvents[i].m_numButtonEvents+args->m_vrControllerEvents[i].m_numMoveEvents) + { + args->m_sendVrControllerEvents[numSendVrControllers++] = + args->m_vrControllerEvents[i]; + + + if (args->m_vrControllerEvents[i].m_numButtonEvents) + { + for (int b=0;bm_vrControllerEvents[i].m_buttons[b] &= eButtonIsDown; + } + } + args->m_vrControllerEvents[i].m_numMoveEvents = 0; + args->m_vrControllerEvents[i].m_numButtonEvents = 0; + } + } + + args->m_cs->unlock(); + + args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents,numSendVrControllers); deltaTimeInSeconds = 0; } @@ -1060,7 +1097,7 @@ m_options(options) #endif m_multiThreadedHelper = helper; - b3Printf("Started PhysicsServer\n"); +// b3Printf("Started PhysicsServer\n"); } @@ -1394,6 +1431,32 @@ extern double gSubStep; extern int gHuskyId; extern btTransform huskyTr; + struct LineSegment + { + btVector3 m_from; + btVector3 m_to; + }; + + struct ColorWidth + { + btVector3FloatData m_color; + int width; + int getHash() const + { + unsigned char r = (unsigned char) m_color.m_floats[0]*255; + unsigned char g = (unsigned char) m_color.m_floats[1]*255; + unsigned char b = (unsigned char) m_color.m_floats[2]*255; + unsigned char w = width; + return r+(256*g)+(256*256*b)+(256*256*256*w); + } + bool equals(const ColorWidth& other) const + { + bool same = ((width == other.width) && (m_color.m_floats[0] == other.m_color.m_floats[0]) && + (m_color.m_floats[1] == other.m_color.m_floats[1]) && + (m_color.m_floats[2] == other.m_color.m_floats[2])); + return same; + } + }; void PhysicsServerExample::drawUserDebugLines() { @@ -1408,6 +1471,14 @@ void PhysicsServerExample::drawUserDebugLines() if (m_multiThreadedHelper) { + + //if gBatchUserDebugLines is true, batch lines based on color+width, to reduce line draw calls + + btAlignedObjectArray< btAlignedObjectArray > sortedIndices; + btAlignedObjectArray< btAlignedObjectArray > sortedLines; + + btHashMap hashedLines; + for (int i = 0; im_userDebugLines.size(); i++) { btVector3 from; @@ -1423,9 +1494,56 @@ void PhysicsServerExample::drawUserDebugLines() color.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0], m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1], m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]); + ColorWidth cw; + color.serializeFloat(cw.m_color); + cw.width = m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth; + int index = -1; + + if (gBatchUserDebugLines) + { + int* indexPtr = hashedLines.find(cw); + if (indexPtr) + { + index = *indexPtr; + } else + { + index = sortedLines.size(); + sortedLines.expand(); + sortedIndices.expand(); + hashedLines.insert(cw,index); + } + btAssert(index>=0); + if (index>=0) + { + btVector3FloatData from1,toX1; + sortedIndices[index].push_back(sortedLines[index].size()); + from.serializeFloat(from1); + sortedLines[index].push_back(from1); + sortedIndices[index].push_back(sortedLines[index].size()); + toX.serializeFloat(toX1); + sortedLines[index].push_back(toX1); + } + } + else + { + m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth); + } + } - m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth); + if (gBatchUserDebugLines) + { + for (int i=0;igetAppInterface()->m_renderer->drawLines(positions,cw.m_color.m_floats,numPoints, stride, indices,numIndices,cw.width); + } } for (int i = 0; im_userDebugText.size(); i++) @@ -1713,11 +1831,35 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt gGraspingController = controllerId; gEnableKukaControl = true; } + + btTransform trLocal; + trLocal.setIdentity(); + trLocal.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI)); + + btTransform trOrg; + trOrg.setIdentity(); + trOrg.setOrigin(btVector3(pos[0], pos[1], pos[2])); + trOrg.setRotation(btQuaternion(orn[0], orn[1], orn[2], orn[3])); + + btTransform tr2a; + tr2a.setIdentity(); + btTransform tr2; + tr2.setIdentity(); + + + + tr2.setOrigin(gVRTeleportPos1); + tr2a.setRotation(gVRTeleportOrn); + + + btTransform trTotal = tr2*tr2a*trOrg*trLocal; + + if (controllerId != gGraspingController) { if (button == 1 && state == 0) { - gResetSimulation = true; + //gResetSimulation = true; //gVRTeleportPos1 = gLastPickPos; } } else @@ -1786,27 +1928,6 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt m_args[0].m_isVrControllerReleasing[controllerId] = (state == 0); } - btTransform trLocal; - trLocal.setIdentity(); - trLocal.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI)); - - btTransform trOrg; - trOrg.setIdentity(); - trOrg.setOrigin(btVector3(pos[0], pos[1], pos[2])); - trOrg.setRotation(btQuaternion(orn[0], orn[1], orn[2], orn[3])); - - btTransform tr2a; - tr2a.setIdentity(); - btTransform tr2; - tr2.setIdentity(); - - - - tr2.setOrigin(gVRTeleportPos1); - tr2a.setRotation(gVRTeleportOrn); - - - btTransform trTotal = tr2*tr2a*trOrg*trLocal; if ((button == 33) || (button == 1)) { @@ -1817,6 +1938,26 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt } } + + m_args[0].m_cs->lock(); + m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId; + m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0]; + m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1]; + m_args[0].m_vrControllerEvents[controllerId].m_pos[2] = trTotal.getOrigin()[2]; + m_args[0].m_vrControllerEvents[controllerId].m_orn[0] = trTotal.getRotation()[0]; + m_args[0].m_vrControllerEvents[controllerId].m_orn[1] = trTotal.getRotation()[1]; + m_args[0].m_vrControllerEvents[controllerId].m_orn[2] = trTotal.getRotation()[2]; + m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3]; + m_args[0].m_vrControllerEvents[controllerId].m_numButtonEvents++; + if (state) + { + m_args[0].m_vrControllerEvents[controllerId].m_buttons[button]|=eButtonIsDown+eButtonTriggered; + } else + { + m_args[0].m_vrControllerEvents[controllerId].m_buttons[button]|=eButtonReleased; + m_args[0].m_vrControllerEvents[controllerId].m_buttons[button] &= ~eButtonIsDown; + } + m_args[0].m_cs->unlock(); } @@ -1868,5 +2009,18 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[ m_args[0].m_vrControllerOrn[controllerId] = trTotal.getRotation(); } + m_args[0].m_cs->lock(); + m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId; + m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0]; + m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1]; + m_args[0].m_vrControllerEvents[controllerId].m_pos[2] = trTotal.getOrigin()[2]; + m_args[0].m_vrControllerEvents[controllerId].m_orn[0] = trTotal.getRotation()[0]; + m_args[0].m_vrControllerEvents[controllerId].m_orn[1] = trTotal.getRotation()[1]; + m_args[0].m_vrControllerEvents[controllerId].m_orn[2] = trTotal.getRotation()[2]; + m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3]; + m_args[0].m_vrControllerEvents[controllerId].m_numMoveEvents++; + m_args[0].m_vrControllerEvents[controllerId].m_analogAxis = analogAxis; + m_args[0].m_cs->unlock(); + } B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc) diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp index 158bdac6b..5c967c9c4 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp @@ -236,9 +236,9 @@ void PhysicsServerSharedMemory::releaseSharedMemory() } -void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec) +void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents) { - m_data->m_commandProcessor->stepSimulationRealTime(dtInSec); + m_data->m_commandProcessor->stepSimulationRealTime(dtInSec,vrEvents, numVREvents); } void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim) diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.h b/examples/SharedMemory/PhysicsServerSharedMemory.h index f29e844f2..bd9b9b152 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.h +++ b/examples/SharedMemory/PhysicsServerSharedMemory.h @@ -26,7 +26,7 @@ public: virtual void processClientCommands(); - virtual void stepSimulationRealTime(double dtInSec); + virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents); virtual void enableRealTimeSimulation(bool enableRealTimeSim); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index d905ca980..acab7d351 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -174,6 +174,18 @@ enum EnumRequestContactDataUpdateFlags CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER = 8, }; +struct RequestRaycastIntersections +{ + double m_rayFromPosition[3]; + double m_rayToPosition[3]; +}; + +struct SendRaycastHits +{ + int m_numRaycastHits; + b3RayHitInfo m_rayHits[MAX_RAY_HITS]; +}; + struct RequestContactDataArgs { int m_startingContactPointIndex; @@ -615,6 +627,7 @@ struct SharedMemoryCommand struct LoadTextureArgs m_loadTextureArguments; struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments; struct UserDebugDrawArgs m_userDebugDrawArgs; + struct RequestRaycastIntersections m_requestRaycastIntersections; struct LoadBunnyArgs m_loadBunnyArguments; }; }; @@ -638,6 +651,14 @@ struct SendOverlappingObjectsArgs int m_numRemainingOverlappingObjects; }; +struct SendVREvents +{ + int m_numVRControllerEvents; + b3VRControllerEvent m_controllerEvents[MAX_VR_CONTROLLERS]; +}; + + + struct SharedMemoryStatus { int m_type; @@ -665,6 +686,8 @@ struct SharedMemoryStatus struct SendVisualShapeDataArgs m_sendVisualShapeArgs; struct UserDebugDrawResultArgs m_userDebugDrawArgs; struct UserConstraintResultArgs m_userConstraintResultArgs; + struct SendVREvents m_sendVREvents; + struct SendRaycastHits m_raycastHits; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index fe79d9dcb..7eb9d37ad 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -35,14 +35,17 @@ enum EnumSharedMemoryClientCommand CMD_CALCULATE_JACOBIAN, CMD_USER_CONSTRAINT, CMD_REQUEST_CONTACT_POINT_INFORMATION, + CMD_REQUEST_RAY_CAST_INTERSECTIONS, + CMD_REQUEST_AABB_OVERLAP, + CMD_SAVE_WORLD, CMD_REQUEST_VISUAL_SHAPE_INFO, CMD_UPDATE_VISUAL_SHAPE, CMD_LOAD_TEXTURE, CMD_SET_SHADOW, CMD_USER_DEBUG_DRAW, - + CMD_REQUEST_VR_EVENTS_DATA, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -107,6 +110,8 @@ enum EnumSharedMemoryServerStatus CMD_USER_DEBUG_DRAW_FAILED, CMD_USER_CONSTRAINT_COMPLETED, CMD_USER_CONSTRAINT_FAILED, + CMD_REQUEST_VR_EVENTS_DATA_COMPLETED, + CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; @@ -199,6 +204,44 @@ struct b3CameraImageData }; +enum b3VREventType +{ + VR_CONTROLLER_MOVE_EVENT=1, + VR_CONTROLLER_BUTTON_EVENT +}; + +#define MAX_VR_BUTTONS 64 +#define MAX_VR_CONTROLLERS 8 +#define MAX_RAY_HITS 128 + +enum b3VRButtonInfo +{ + eButtonIsDown = 1, + eButtonTriggered = 2, + eButtonReleased = 4, +}; + +struct b3VRControllerEvent +{ + int m_controllerId;//valid for VR_CONTROLLER_MOVE_EVENT and VR_CONTROLLER_BUTTON_EVENT + int m_numMoveEvents; + int m_numButtonEvents; + + float m_pos[4];//valid for VR_CONTROLLER_MOVE_EVENT and VR_CONTROLLER_BUTTON_EVENT + float m_orn[4];//valid for VR_CONTROLLER_MOVE_EVENT and VR_CONTROLLER_BUTTON_EVENT + + float m_analogAxis;//valid if VR_CONTROLLER_MOVE_EVENT + + int m_buttons[MAX_VR_BUTTONS];//valid if VR_CONTROLLER_BUTTON_EVENT, see b3VRButtonInfo +}; + +struct b3VREventsData +{ + int m_numControllerEvents; + struct b3VRControllerEvent* m_controllerEvents; +}; + + struct b3ContactPointData { //todo: expose some contact flags, such as telling which fields below are valid @@ -237,6 +280,22 @@ struct b3ContactInformation struct b3ContactPointData* m_contactPointData; }; +struct b3RayHitInfo +{ + double m_hitFraction; + int m_hitObjectUniqueId; + int m_hitObjectLinkIndex; + double m_hitPositionWorld[3]; + double m_hitNormalWorld[3]; +}; + +struct b3RaycastInformation +{ + int m_numRayHits; + struct b3RayHitInfo* m_rayHits; +}; + + #define VISUAL_SHAPE_MAX_PATH_LEN 128 struct b3VisualShapeData diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index 1a840b834..71a1aa7e9 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -21,6 +21,7 @@ #include "LinearMath/btIDebugDraw.h" int gSharedMemoryKey = -1; int gDebugDrawFlags = 0; +bool gDisplayDistortion = false; //how can you try typing on a keyboard, without seeing it? //it is pretty funny, to see the desktop in VR! @@ -704,7 +705,7 @@ bool CMainApplication::HandleInput() // printf("Device PRESSED: %d, button %d\n", unDevice, button); if (button==2) { - glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + //glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); ///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync ///so it can (and likely will) cause crashes ///add a special debug drawer that deals with this @@ -831,15 +832,31 @@ void CMainApplication::RenderFrame() DrawControllers(); } RenderStereoTargets(); + + if (gDisplayDistortion) { B3_PROFILE("RenderDistortion"); RenderDistortion(); + } else + { + glBindFramebuffer( GL_FRAMEBUFFER, 0 ); + glDisable( GL_MULTISAMPLE ); + glBindFramebuffer(GL_READ_FRAMEBUFFER, rightEyeDesc.m_nRenderFramebufferId ); + glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0); + + glBlitFramebuffer( 0, 0, m_nRenderWidth, m_nRenderHeight, 0, 0, m_nRenderWidth, m_nRenderHeight, + GL_COLOR_BUFFER_BIT, + GL_LINEAR ); + + glBindFramebuffer(GL_READ_FRAMEBUFFER, 0); + glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0 ); } vr::Texture_t leftEyeTexture = {(void*)leftEyeDesc.m_nResolveTextureId, vr::API_OpenGL, vr::ColorSpace_Gamma }; vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTexture ); vr::Texture_t rightEyeTexture = {(void*)rightEyeDesc.m_nResolveTextureId, vr::API_OpenGL, vr::ColorSpace_Gamma }; vr::VRCompositor()->Submit(vr::Eye_Right, &rightEyeTexture ); + } if ( m_bVblank && m_bGlFinishHack ) 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/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d130712d5..5928d357d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1956,6 +1956,188 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args return Py_None; } + +static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keywds) +{ + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + PyObject* rayFromObj=0; + PyObject* rayToObj=0; + double from[3]; + double to[3]; + static char *kwlist[] = { "rayFromPosition", "rayToPosition", NULL }; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO", kwlist, + &rayFromObj, &rayToObj)) + return NULL; + + pybullet_internalSetVectord(rayFromObj,from); + pybullet_internalSetVectord(rayToObj,to); + + commandHandle = b3CreateRaycastCommandInit(sm, from[0],from[1],from[2], + to[0],to[1],to[2]); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType==CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED) + { + struct b3RaycastInformation raycastInfo; + PyObject* rayHitsObj = 0; + int i; + b3GetRaycastInformation(sm, &raycastInfo); + + rayHitsObj = PyTuple_New(raycastInfo.m_numRayHits); + for (i=0;i20): + widths[e[CONTROLLER_ID]] = 1 + if (e[BUTTONS][1]&p.VR_BUTTON_WAS_TRIGGERED): + p.resetSimulation() + #p.setGravity(0,0,-10) + p.removeAllUserDebugItems() + p.loadURDF("plane.urdf") + if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN): + pt = prevPosition[e[CONTROLLER_ID]] + + #print(prevPosition[e[0]]) + #print(e[1]) + diff = [pt[0]-e[POSITION][0],pt[1]-e[POSITION][1],pt[2]-e[POSITION][2]] + lenSqr = diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2] + ptDistThreshold = 0.01 + if (lenSqr>(ptDistThreshold*ptDistThreshold)): + p.addUserDebugLine(e[POSITION],prevPosition[e[CONTROLLER_ID]],colors[e[CONTROLLER_ID]],widths[e[CONTROLLER_ID]]) + #p.loadURDF("cube_small.urdf",e[1]) + colors[e[CONTROLLER_ID]] = [1-colors[e[CONTROLLER_ID]][0],1-colors[e[CONTROLLER_ID]][1],1-colors[e[CONTROLLER_ID]][2]] + prevPosition[e[CONTROLLER_ID]] = e[POSITION] \ No newline at end of file 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..522dd8bd1 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__ @@ -30,6 +28,9 @@ #if defined (SUNOS) || defined (__SUNOS__) #include #endif +#ifdef __APPLE__ +#include +#endif #if defined(WIN32) || defined(_WIN32) @@ -55,6 +56,12 @@ #else //_WIN32 #include + +#ifdef BT_LINUX_REALTIME +//required linking against rt (librt) +#include +#endif //BT_LINUX_REALTIME + #endif //_WIN32 #define mymin(a,b) (a > b ? a : b) @@ -65,12 +72,14 @@ struct btClockData #ifdef BT_USE_WINDOWS_TIMERS LARGE_INTEGER mClockFrequency; LONGLONG mStartTick; - LONGLONG mPrevElapsedTime; LARGE_INTEGER mStartTime; #else #ifdef __CELLOS_LV2__ uint64_t mStartTime; #else +#ifdef __APPLE__ + uint64_t mStartTimeNano; +#endif struct timeval mStartTime; #endif #endif //__CELLOS_LV2__ @@ -111,7 +120,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__ @@ -121,6 +129,9 @@ void btClock::reset() SYS_TIMEBASE_GET( newTime ); m_data->mStartTime = newTime; #else +#ifdef __APPLE__ + m_data->mStartTimeNano = mach_absolute_time(); +#endif gettimeofday(&m_data->mStartTime, 0); #endif #endif @@ -128,7 +139,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 +149,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 +174,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 +208,66 @@ 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 +#ifdef __APPLE__ + uint64_t ticks = mach_absolute_time() - m_data->mStartTimeNano; + static long double conversion = 0.0L; + if( 0.0L == conversion ) + { + // attempt to get conversion to nanoseconds + mach_timebase_info_data_t info; + int err = mach_timebase_info( &info ); + if( err ) + { + btAssert(0); + conversion = 1.; + } + conversion = info.numer / info.denom; + } + return (ticks * conversion); + + +#else//__APPLE__ + +#ifdef BT_LINUX_REALTIME + timespec ts; + clock_gettime(CLOCK_REALTIME,&ts); + return 1000000000*ts.tv_sec + ts.tv_nsec; +#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)*1000; +#endif //BT_LINUX_REALTIME + +#endif//__APPLE__ +#endif//__CELLOS_LV2__ +#endif +} /// Returns the time in s since the last call to reset or since @@ -370,6 +398,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 +466,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 +559,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 +587,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 +676,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