From af0e1d58ef80080bcf9281325563403465ce68ee Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 17 Jun 2017 10:24:47 -0700 Subject: [PATCH 1/4] obj2sdf -> don't crash when no name is given, remind user of --fileName=... --- Extras/obj2sdf/obj2sdf.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Extras/obj2sdf/obj2sdf.cpp b/Extras/obj2sdf/obj2sdf.cpp index 7264d704b..488b24d54 100644 --- a/Extras/obj2sdf/obj2sdf.cpp +++ b/Extras/obj2sdf/obj2sdf.cpp @@ -40,7 +40,11 @@ int main(int argc, char* argv[]) b3CommandLineArgs args(argc,argv); char* fileName; args.GetCmdLineArgument("fileName",fileName); - + if (fileName==0) + { + printf("required --fileName=\"name\""); + exit(0); + } std::string matLibName = StripExtension(fileName); printf("fileName = %s\n", fileName); From 3a826a5997ba299be25773af46fff10ad707c4d6 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 17 Jun 2017 11:21:10 -0700 Subject: [PATCH 2/4] getCameraImage: use debug visualizer camera viewmatrix/projection matrix if possible (only if view/proj matrix is not provided) --- .../PhysicsServerCommandProcessor.cpp | 24 +++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6354e8fe0..776e70016 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3066,9 +3066,29 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_requestPixelDataArguments.m_projectionMatrix); } else { - m_data->m_visualConverter.render(); + SharedMemoryStatus tmpCmd = serverStatusOut; + bool result = this->m_data->m_guiHelper->getCameraInfo( + &tmpCmd.m_visualizerCameraResultArgs.m_width, + &tmpCmd.m_visualizerCameraResultArgs.m_height, + tmpCmd.m_visualizerCameraResultArgs.m_viewMatrix, + tmpCmd.m_visualizerCameraResultArgs.m_projectionMatrix, + tmpCmd.m_visualizerCameraResultArgs.m_camUp, + tmpCmd.m_visualizerCameraResultArgs.m_camForward, + tmpCmd.m_visualizerCameraResultArgs.m_horizontal, + tmpCmd.m_visualizerCameraResultArgs.m_vertical, + &tmpCmd.m_visualizerCameraResultArgs.m_yaw, + &tmpCmd.m_visualizerCameraResultArgs.m_pitch, + &tmpCmd.m_visualizerCameraResultArgs.m_dist, + tmpCmd.m_visualizerCameraResultArgs.m_target); + if (result) + { + m_data->m_visualConverter.render(tmpCmd.m_visualizerCameraResultArgs.m_viewMatrix, + tmpCmd.m_visualizerCameraResultArgs.m_projectionMatrix); + } else + { + m_data->m_visualConverter.render(); + } } - } m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels, From 2e6f8c271e29900545ce5550da19fae70695a9b7 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 17 Jun 2017 13:29:14 -0700 Subject: [PATCH 3/4] allow to disable/enable default keyboard shortcuts ('w', 'd' 's' etc) and default mouse picking pybullet.getMouseEvents / b3RequestMouseEventsCommandInit --- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 15 +- examples/SharedMemory/PhysicsClient.h | 2 + examples/SharedMemory/PhysicsClientC_API.cpp | 25 ++ examples/SharedMemory/PhysicsClientC_API.h | 4 + .../PhysicsClientSharedMemory.cpp | 25 ++ .../SharedMemory/PhysicsClientSharedMemory.h | 2 + .../PhysicsCommandProcessorInterface.h | 2 +- examples/SharedMemory/PhysicsDirect.cpp | 24 ++ examples/SharedMemory/PhysicsDirect.h | 2 + examples/SharedMemory/PhysicsLoopBack.cpp | 5 + examples/SharedMemory/PhysicsLoopBack.h | 2 + .../PhysicsServerCommandProcessor.cpp | 61 ++++- .../PhysicsServerCommandProcessor.h | 3 +- .../SharedMemory/PhysicsServerExample.cpp | 222 ++++++++++++------ .../PhysicsServerSharedMemory.cpp | 4 +- .../SharedMemory/PhysicsServerSharedMemory.h | 2 +- examples/SharedMemory/SharedMemoryCommands.h | 8 +- examples/SharedMemory/SharedMemoryPublic.h | 28 ++- examples/pybullet/pybullet.c | 51 +++- 19 files changed, 401 insertions(+), 86 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index cdfef5404..b4e7ea3d6 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -130,6 +130,10 @@ static bool renderGrid = true; bool renderGui = true; static bool enable_experimental_opencl = false; +static bool gEnableDefaultKeyboardShortcuts = true; +static bool gEnableDefaultMousePicking = true; + + int gDebugDrawFlags = 0; static bool pauseSimulation=false; static bool singleStepSimulation = false; @@ -200,7 +204,7 @@ void MyKeyboardCallback(int key, int state) //if (handled) // return; - //if (s_window && s_window->isModifierKeyPressed(B3G_CONTROL)) + if (gEnableDefaultKeyboardShortcuts) { if (key=='a' && state) { @@ -376,6 +380,15 @@ void OpenGLExampleBrowserVisualizerFlagCallback(int flag, bool enable) renderGrid = enable; } + if (flag == COV_ENABLE_KEYBOARD_SHORTCUTS) + { + gEnableDefaultKeyboardShortcuts = enable; + } + if (flag == COV_ENABLE_MOUSE_PICKING) + { + gEnableDefaultMousePicking = enable; + } + if (flag == COV_ENABLE_WIREFRAME) { visualWireframe = enable; diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 2ce391c60..28b823008 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -62,6 +62,8 @@ public: virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData) = 0; + virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData) = 0; + virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits) = 0; virtual void setTimeOut(double timeOutInSeconds) = 0; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 47be09e4c..de2b3a8f6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3208,6 +3208,31 @@ void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3Keyboard } } +b3SharedMemoryCommandHandle b3RequestMouseEventsCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_REQUEST_MOUSE_EVENTS_DATA; + command->m_updateFlags = 0; + + return (b3SharedMemoryCommandHandle)command; +} + +void b3GetMouseEventsData(b3PhysicsClientHandle physClient, struct b3MouseEventsData* mouseEventsData) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + if (cl) + { + cl->getCachedMouseEvents(mouseEventsData); + } +} + + + b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index ee82cfbec..267691dd3 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -433,6 +433,10 @@ int b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle, i b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit(b3PhysicsClientHandle physClient); void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3KeyboardEventsData* keyboardEventsData); +b3SharedMemoryCommandHandle b3RequestMouseEventsCommandInit(b3PhysicsClientHandle physClient); +void b3GetMouseEventsData(b3PhysicsClientHandle physClient, struct b3MouseEventsData* mouseEventsData); + + b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient); int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName); int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 66ef8b435..0877b1d0a 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -46,6 +46,8 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_cachedVisualShapes; btAlignedObjectArray m_cachedVREvents; btAlignedObjectArray m_cachedKeyboardEvents; + btAlignedObjectArray m_cachedMouseEvents; + btAlignedObjectArray m_raycastHits; btAlignedObjectArray m_bodyIdsRequestInfo; @@ -876,6 +878,21 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { break; } + case CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED: + { + B3_PROFILE("CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED"); + if (m_data->m_verboseOutput) + { + b3Printf("Request mouse events completed"); + } + m_data->m_cachedMouseEvents.resize(serverCmd.m_sendMouseEvents.m_numMouseEvents); + for (int i=0;im_cachedMouseEvents[i] = serverCmd.m_sendMouseEvents.m_mouseEvents[i]; + } + break; + } + case CMD_REQUEST_AABB_OVERLAP_COMPLETED: { B3_PROFILE("CMD_REQUEST_AABB_OVERLAP_COMPLETED"); @@ -1393,6 +1410,14 @@ void PhysicsClientSharedMemory::getCachedKeyboardEvents(struct b3KeyboardEventsD &m_data->m_cachedKeyboardEvents[0] : 0; } +void PhysicsClientSharedMemory::getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData) +{ + mouseEventsData->m_numMouseEvents = m_data->m_cachedMouseEvents.size(); + mouseEventsData->m_mouseEvents = mouseEventsData->m_numMouseEvents? + &m_data->m_cachedMouseEvents[0] : 0; +} + + void PhysicsClientSharedMemory::getCachedRaycastHits(struct b3RaycastInformation* raycastHits) { raycastHits->m_numRayHits = m_data->m_raycastHits.size(); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index f44e438e8..31c717c02 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -72,6 +72,8 @@ public: virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData); + virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData); + virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); virtual void setTimeOut(double timeOutInSeconds); diff --git a/examples/SharedMemory/PhysicsCommandProcessorInterface.h b/examples/SharedMemory/PhysicsCommandProcessorInterface.h index 609d04cd3..1614cc33a 100644 --- a/examples/SharedMemory/PhysicsCommandProcessorInterface.h +++ b/examples/SharedMemory/PhysicsCommandProcessorInterface.h @@ -40,7 +40,7 @@ public: virtual ~CommandProcessorInterface(){} virtual void syncPhysicsToGraphics()=0; - virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents)=0; + virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents)=0; virtual void enableRealTimeSimulation(bool enableRealTimeSim)=0; virtual bool isRealTimeSimulationEnabled() const=0; diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index cafcd5449..afbf92f0a 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -54,6 +54,7 @@ struct PhysicsDirectInternalData btAlignedObjectArray m_cachedVREvents; btAlignedObjectArray m_cachedKeyboardEvents; + btAlignedObjectArray m_cachedMouseEvents; btAlignedObjectArray m_raycastHits; @@ -699,6 +700,21 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd break; } + case CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED: + { + B3_PROFILE("CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED"); + if (m_data->m_verboseOutput) + { + b3Printf("Request mouse events completed"); + } + m_data->m_cachedMouseEvents.resize(serverCmd.m_sendMouseEvents.m_numMouseEvents); + for (int i=0;im_cachedMouseEvents[i] = serverCmd.m_sendMouseEvents.m_mouseEvents[i]; + } + break; + } + case CMD_REQUEST_INTERNAL_DATA_COMPLETED: { if (serverCmd.m_numDataStreamBytes) @@ -1159,6 +1175,14 @@ void PhysicsDirect::getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboar &m_data->m_cachedKeyboardEvents[0] : 0; } +void PhysicsDirect::getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData) +{ + mouseEventsData->m_numMouseEvents = m_data->m_cachedMouseEvents.size(); + mouseEventsData->m_mouseEvents = mouseEventsData->m_numMouseEvents? + &m_data->m_cachedMouseEvents[0] : 0; +} + + void PhysicsDirect::getCachedRaycastHits(struct b3RaycastInformation* raycastHits) { raycastHits->m_numRayHits = m_data->m_raycastHits.size(); diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 64ea073eb..282a61732 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -95,6 +95,8 @@ public: virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData); + virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData); + virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); //the following APIs are for internal use for visualization: diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index d8eac2542..3067f2ace 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -190,6 +190,11 @@ void PhysicsLoopBack::getCachedKeyboardEvents(struct b3KeyboardEventsData* keybo return m_data->m_physicsClient->getCachedKeyboardEvents(keyboardEventsData); } +void PhysicsLoopBack::getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData) +{ + return m_data->m_physicsClient->getCachedMouseEvents(mouseEventsData); +} + void PhysicsLoopBack::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects) { return m_data->m_physicsClient->getCachedOverlappingObjects(overlappingObjects); diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index cd05027f1..f83cd7234 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -76,6 +76,8 @@ public: virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData); + virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData); + virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); virtual void setTimeOut(double timeOutInSeconds); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 776e70016..f3d0f40b9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1153,7 +1153,8 @@ struct PhysicsServerCommandProcessorInternalData btAlignedObjectArray m_keyboardEvents; - + btAlignedObjectArray m_mouseEvents; + CommandLogger* m_commandLogger; CommandLogPlayback* m_logPlayback; @@ -2774,6 +2775,27 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; }; + case CMD_REQUEST_MOUSE_EVENTS_DATA: + { + + serverStatusOut.m_sendMouseEvents.m_numMouseEvents = m_data->m_mouseEvents.size(); + if (serverStatusOut.m_sendMouseEvents.m_numMouseEvents>MAX_MOUSE_EVENTS) + { + serverStatusOut.m_sendMouseEvents.m_numMouseEvents = MAX_MOUSE_EVENTS; + } + for (int i=0;im_mouseEvents[i]; + } + + m_data->m_mouseEvents.resize(0); + serverStatusOut.m_type = CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED; + hasStatus = true; + break; + }; + + + case CMD_REQUEST_KEYBOARD_EVENTS_DATA: { //BT_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA"); @@ -7032,7 +7054,7 @@ bool PhysicsServerCommandProcessor::isRealTimeSimulationEnabled() const return m_data->m_allowRealTimeSimulation; } -void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents,const struct b3KeyboardEvent* keyEvents, int numKeyEvents) +void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents) { m_data->m_vrControllerEvents.addNewVREvents(vrControllerEvents,numVRControllerEvents); for (int i=0;im_stateLoggers.size();i++) @@ -7044,6 +7066,41 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const } } + for (int ii=0;iim_mouseEvents.size();e++) + { + if (event.m_eventType == m_data->m_mouseEvents[e].m_eventType) + { + if (event.m_eventType == MOUSE_MOVE_EVENT) + { + m_data->m_mouseEvents[e].m_mousePosX = event.m_mousePosX; + m_data->m_mouseEvents[e].m_mousePosY = event.m_mousePosY; + found = true; + } else + if ((event.m_eventType == MOUSE_BUTTON_EVENT) && event.m_buttonIndex == m_data->m_mouseEvents[e].m_buttonIndex) + { + m_data->m_mouseEvents[e].m_buttonState |= event.m_buttonState; + if (event.m_buttonState & eButtonIsDown) + { + m_data->m_mouseEvents[e].m_buttonState |= eButtonIsDown; + } else + { + m_data->m_mouseEvents[e].m_buttonState &= ~eButtonIsDown; + } + found = true; + } + } + } + if (!found) + { + m_data->m_mouseEvents.push_back(event); + } + } + for (int i=0;i m_keyboardEvents; btAlignedObjectArray m_sendKeyEvents; - + btAlignedObjectArray m_allMouseEvents; + btAlignedObjectArray m_sendMouseEvents; PhysicsServerSharedMemory* m_physicsServerPtr; b3AlignedObjectArray m_positions; @@ -389,8 +393,52 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) b3KeyboardEvent* keyEvents = args->m_sendKeyEvents.size()? &args->m_sendKeyEvents[0] : 0; args->m_csGUI->unlock(); + + args->m_csGUI->lock(); + if (gEnableDefaultMousePicking) { - args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents,numSendVrControllers, keyEvents, args->m_sendKeyEvents.size()); + for (int i = 0; i < args->m_mouseCommands.size(); i++) + { + switch (args->m_mouseCommands[i].m_type) + { + case MyMouseMove: + { + args->m_physicsServerPtr->movePickedBody(args->m_mouseCommands[i].m_rayFrom, args->m_mouseCommands[i].m_rayTo); + break; + }; + case MyMouseButtonDown: + { + args->m_physicsServerPtr->pickBody(args->m_mouseCommands[i].m_rayFrom, args->m_mouseCommands[i].m_rayTo); + break; + } + case MyMouseButtonUp: + { + args->m_physicsServerPtr->removePickingConstraint(); + break; + } + + default: + { + } + } + } + } + + + args->m_sendMouseEvents.resize(args->m_allMouseEvents.size()); + for (int i=0;im_allMouseEvents.size();i++) + { + args->m_sendMouseEvents[i] = args->m_allMouseEvents[i]; + } + b3MouseEvent* mouseEvents = args->m_sendMouseEvents.size()? &args->m_sendMouseEvents[0] : 0; + + args->m_allMouseEvents.clear(); + args->m_mouseCommands.clear(); + args->m_csGUI->unlock(); + + + { + args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents,numSendVrControllers, keyEvents, args->m_sendKeyEvents.size(), mouseEvents, args->m_sendMouseEvents.size()); } { if (gEnableUpdateDebugDrawLines) @@ -405,35 +453,7 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) } - args->m_csGUI->lock(); - for (int i = 0; i < args->m_mouseCommands.size(); i++) - { - switch (args->m_mouseCommands[i].m_type) - { - case MyMouseMove: - { - args->m_physicsServerPtr->movePickedBody(args->m_mouseCommands[i].m_rayFrom, args->m_mouseCommands[i].m_rayTo); - break; - }; - case MyMouseButtonDown: - { - args->m_physicsServerPtr->pickBody(args->m_mouseCommands[i].m_rayFrom, args->m_mouseCommands[i].m_rayTo); - break; - } - case MyMouseButtonUp: - { - args->m_physicsServerPtr->removePickingConstraint(); - break; - } - - default: - { - } - - } - } - args->m_mouseCommands.clear(); - args->m_csGUI->unlock(); + { args->m_physicsServerPtr->processClientCommands(); @@ -1282,6 +1302,16 @@ public: return false; } + b3MouseEvent event; + event.m_buttonState = 0; + event.m_buttonIndex = -1; + event.m_mousePosX = x; + event.m_mousePosY = y; + event.m_eventType = MOUSE_MOVE_EVENT; + m_args[0].m_csGUI->lock(); + m_args[0].m_allMouseEvents.push_back(event); + m_args[0].m_csGUI->unlock(); + btVector3 rayTo = getRayTo(int(x), int(y)); btVector3 rayFrom; renderer->getActiveCamera()->getCameraPosition(rayFrom); @@ -1311,6 +1341,22 @@ public: CommonWindowInterface* window = m_guiHelper->getAppInterface()->m_window; + b3MouseEvent event; + event.m_buttonIndex = button; + event.m_mousePosX = x; + event.m_mousePosY = y; + event.m_eventType = MOUSE_BUTTON_EVENT; + if (state) + { + event.m_buttonState = eButtonIsDown + eButtonTriggered; + } else + { + event.m_buttonState = eButtonReleased; + } + + m_args[0].m_csGUI->lock(); + m_args[0].m_allMouseEvents.push_back(event); + m_args[0].m_csGUI->unlock(); if (state==1) { @@ -1415,50 +1461,52 @@ public: btVector3 VRTeleportPos =this->m_physicsServer.getVRTeleportPosition(); - if (key=='w' && state) + if (gEnableDefaultKeyboardShortcuts) { - VRTeleportPos[0]+=shift; - m_physicsServer.setVRTeleportPosition(VRTeleportPos); - saveCurrentSettingsVR(VRTeleportPos); + if (key=='w' && state) + { + VRTeleportPos[0]+=shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); + } + if (key=='s' && state) + { + VRTeleportPos[0]-=shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); + } + if (key=='a' && state) + { + VRTeleportPos[1]-=shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); + } + if (key=='d' && state) + { + VRTeleportPos[1]+=shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); + } + if (key=='q' && state) + { + VRTeleportPos[2]+=shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); + } + if (key=='e' && state) + { + VRTeleportPos[2]-=shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); + } + if (key=='z' && state) + { + gVRTeleportRotZ+=shift; + btQuaternion VRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ); + m_physicsServer.setVRTeleportOrientation(VRTeleportOrn); + saveCurrentSettingsVR(VRTeleportPos); + } } - if (key=='s' && state) - { - VRTeleportPos[0]-=shift; - m_physicsServer.setVRTeleportPosition(VRTeleportPos); - saveCurrentSettingsVR(VRTeleportPos); - } - if (key=='a' && state) - { - VRTeleportPos[1]-=shift; - m_physicsServer.setVRTeleportPosition(VRTeleportPos); - saveCurrentSettingsVR(VRTeleportPos); - } - if (key=='d' && state) - { - VRTeleportPos[1]+=shift; - m_physicsServer.setVRTeleportPosition(VRTeleportPos); - saveCurrentSettingsVR(VRTeleportPos); - } - if (key=='q' && state) - { - VRTeleportPos[2]+=shift; - m_physicsServer.setVRTeleportPosition(VRTeleportPos); - saveCurrentSettingsVR(VRTeleportPos); - } - if (key=='e' && state) - { - VRTeleportPos[2]-=shift; - m_physicsServer.setVRTeleportPosition(VRTeleportPos); - saveCurrentSettingsVR(VRTeleportPos); - } - if (key=='z' && state) - { - gVRTeleportRotZ+=shift; - btQuaternion VRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ); - m_physicsServer.setVRTeleportOrientation(VRTeleportOrn); - saveCurrentSettingsVR(VRTeleportPos); - } - return false; @@ -1514,8 +1562,22 @@ public: m_physicsServer.enableRealTimeSimulation(true); } - - + if (args.CheckCmdLineFlag("disableDefaultKeyboardShortcuts")) + { + gEnableDefaultKeyboardShortcuts = false; + } + if (args.CheckCmdLineFlag("enableDefaultKeyboardShortcuts")) + { + gEnableDefaultKeyboardShortcuts = true; + } + if (args.CheckCmdLineFlag("disableDefaultMousePicking")) + { + gEnableDefaultMousePicking = false; + } + if (args.CheckCmdLineFlag("enableDefaultMousePicking")) + { + gEnableDefaultMousePicking = true; + } } @@ -1797,6 +1859,16 @@ void PhysicsServerExample::updateGraphics() gEnableRendering = (enable!=0); } + if (flag == COV_ENABLE_KEYBOARD_SHORTCUTS) + { + gEnableDefaultKeyboardShortcuts = (enable!=0); + } + + if (flag == COV_ENABLE_MOUSE_PICKING) + { + gEnableDefaultMousePicking = (enable!=0); + } + m_multiThreadedHelper->m_childGuiHelper->setVisualizerFlag(m_multiThreadedHelper->m_visualizerFlag,m_multiThreadedHelper->m_visualizerEnable); m_multiThreadedHelper->mainThreadRelease(); break; diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp index 8eea09002..22fb5b577 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp @@ -226,9 +226,9 @@ void PhysicsServerSharedMemory::releaseSharedMemory() } -void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents) +void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents) { - m_data->m_commandProcessor->stepSimulationRealTime(dtInSec,vrEvents, numVREvents, keyEvents,numKeyEvents); + m_data->m_commandProcessor->stepSimulationRealTime(dtInSec,vrEvents, numVREvents, keyEvents,numKeyEvents,mouseEvents, numMouseEvents); } void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim) diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.h b/examples/SharedMemory/PhysicsServerSharedMemory.h index b53df5537..513ddb939 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.h +++ b/examples/SharedMemory/PhysicsServerSharedMemory.h @@ -27,7 +27,7 @@ public: virtual void processClientCommands(); - virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents); + virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents); virtual void enableRealTimeSimulation(bool enableRealTimeSim); virtual bool isRealTimeSimulationEnabled() const; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 27ae33970..cfe3db329 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -721,6 +721,11 @@ struct SendKeyboardEvents b3KeyboardEvent m_keyboardEvents[MAX_KEYBOARD_EVENTS]; }; +struct SendMouseEvents +{ + int m_numMouseEvents; + b3MouseEvent m_mouseEvents[MAX_MOUSE_EVENTS]; +}; enum eVRCameraEnums { @@ -931,7 +936,6 @@ struct SharedMemoryCommand struct b3CreateVisualShapeArgs m_createVisualShapeArgs; struct b3CreateMultiBodyArgs m_createMultiBodyArgs; struct b3RequestCollisionInfoArgs m_requestCollisionInfoArgs; - }; }; @@ -1001,6 +1005,8 @@ struct SharedMemoryStatus struct b3CreateVisualShapeResultArgs m_createVisualShapeResultArgs; struct b3CreateMultiBodyResultArgs m_createMultiBodyResultArgs; struct b3SendCollisionInfoArgs m_sendCollisionInfoArgs; + struct SendMouseEvents m_sendMouseEvents; + }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 9d1f92507..b5526bcd5 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -66,6 +66,7 @@ enum EnumSharedMemoryClientCommand CMD_CREATE_VISUAL_SHAPE, CMD_CREATE_MULTI_BODY, CMD_REQUEST_COLLISION_INFO, + CMD_REQUEST_MOUSE_EVENTS_DATA, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -159,6 +160,7 @@ enum EnumSharedMemoryServerStatus CMD_CREATE_MULTI_BODY_COMPLETED, CMD_REQUEST_COLLISION_INFO_COMPLETED, CMD_REQUEST_COLLISION_INFO_FAILED, + CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; @@ -325,7 +327,7 @@ enum b3VREventType #define MAX_RAY_INTERSECTION_BATCH_SIZE 256 #define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE #define MAX_KEYBOARD_EVENTS 256 - +#define MAX_MOUSE_EVENTS 256 enum b3VRButtonInfo @@ -384,6 +386,28 @@ struct b3KeyboardEventsData struct b3KeyboardEvent* m_keyboardEvents; }; + +enum eMouseEventTypeEnums +{ + MOUSE_MOVE_EVENT=1, + MOUSE_BUTTON_EVENT=2, +}; + +struct b3MouseEvent +{ + int m_eventType; + float m_mousePosX; + float m_mousePosY; + int m_buttonIndex; + int m_buttonState; +}; + +struct b3MouseEventsData +{ + int m_numMouseEvents; + struct b3MouseEvent* m_mouseEvents; +}; + struct b3ContactPointData { //todo: expose some contact flags, such as telling which fields below are valid @@ -531,6 +555,8 @@ enum b3ConfigureDebugVisualizerEnum COV_ENABLE_VR_RENDER_CONTROLLERS, COV_ENABLE_RENDERING, COV_ENABLE_SYNC_RENDERING_INTERNAL, + COV_ENABLE_KEYBOARD_SHORTCUTS, + COV_ENABLE_MOUSE_PICKING, }; enum b3AddUserDebugItemEnum diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index bbcc8c3cb..e75e6c1b7 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3996,6 +3996,50 @@ static PyObject* pybullet_getKeyboardEvents(PyObject* self, PyObject* args, PyOb return keyEventsObj; } +static PyObject* pybullet_getMouseEvents(PyObject* self, PyObject* args, PyObject* keywds) +{ + b3SharedMemoryCommandHandle commandHandle; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + struct b3MouseEventsData mouseEventsData; + PyObject* mouseEventsObj = 0; + int i = 0; + + static char* kwlist[] = {"physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + commandHandle = b3RequestMouseEventsCommandInit(sm); + b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + b3GetMouseEventsData(sm, &mouseEventsData); + + mouseEventsObj = PyTuple_New(mouseEventsData.m_numMouseEvents); + + for (i = 0; i < mouseEventsData.m_numMouseEvents; i++) + { + PyObject* mouseEventObj = PyTuple_New(5); + PyTuple_SetItem(mouseEventObj,0, PyInt_FromLong(mouseEventsData.m_mouseEvents[i].m_eventType)); + PyTuple_SetItem(mouseEventObj,1, PyFloat_FromDouble(mouseEventsData.m_mouseEvents[i].m_mousePosX)); + PyTuple_SetItem(mouseEventObj,2, PyFloat_FromDouble(mouseEventsData.m_mouseEvents[i].m_mousePosY)); + PyTuple_SetItem(mouseEventObj,3, PyInt_FromLong(mouseEventsData.m_mouseEvents[i].m_buttonIndex)); + PyTuple_SetItem(mouseEventObj,4, PyInt_FromLong(mouseEventsData.m_mouseEvents[i].m_buttonState)); + PyTuple_SetItem(mouseEventsObj,i,mouseEventObj); + + } + return mouseEventsObj; +} + + + static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject* keywds) { b3SharedMemoryCommandHandle commandHandle; @@ -6756,7 +6800,10 @@ static PyMethodDef SpamMethods[] = { "Set properties of the VR Camera such as its root transform " "for teleporting or to track objects (camera inside a vehicle for example)."}, {"getKeyboardEvents", (PyCFunction)pybullet_getKeyboardEvents, METH_VARARGS | METH_KEYWORDS, - "Get Keyboard events, keycode and state (KEY_IS_DOWN, KEY_WAS_TRIGGER, KEY_WAS_RELEASED)"}, + "Get keyboard events, keycode and state (KEY_IS_DOWN, KEY_WAS_TRIGGERED, KEY_WAS_RELEASED)"}, + + {"getMouseEvents", (PyCFunction)pybullet_getMouseEvents, METH_VARARGS | METH_KEYWORDS, + "Get mouse events, event type and button state (KEY_IS_DOWN, KEY_WAS_TRIGGERED, KEY_WAS_RELEASED)"}, {"startStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS, "Start logging of state, such as robot base position, orientation, joint positions etc. " @@ -6923,6 +6970,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "COV_ENABLE_VR_TELEPORTING", COV_ENABLE_VR_TELEPORTING); PyModule_AddIntConstant(m, "COV_ENABLE_RENDERING", COV_ENABLE_RENDERING); PyModule_AddIntConstant(m, "COV_ENABLE_VR_RENDER_CONTROLLERS", COV_ENABLE_VR_RENDER_CONTROLLERS); + PyModule_AddIntConstant(m, "COV_ENABLE_KEYBOARD_SHORTCUTS", COV_ENABLE_KEYBOARD_SHORTCUTS); + PyModule_AddIntConstant(m, "COV_ENABLE_MOUSE_PICKING", COV_ENABLE_MOUSE_PICKING); PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER); From e5db5192f09f45453703e4afcb9bf5d4abeeb526 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 17 Jun 2017 13:34:03 -0700 Subject: [PATCH 4/4] bump up pybullet version again --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 27865edf2..d146cecc6 100644 --- a/setup.py +++ b/setup.py @@ -419,7 +419,7 @@ else: setup( name = 'pybullet', - version='1.1.7', + version='1.1.8', description='Official Python Interface for the Bullet Physics SDK Robotics Simulator', long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3',