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); diff --git a/docs/pybullet_quickstartguide.pdf b/docs/pybullet_quickstartguide.pdf index d1305dd45..ebedaed80 100644 Binary files a/docs/pybullet_quickstartguide.pdf and b/docs/pybullet_quickstartguide.pdf differ 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..84f856467 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -721,7 +721,8 @@ b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHan } return 0; } -void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius) + +int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -735,11 +736,13 @@ void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,d command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_sphereRadius = radius; command->m_createCollisionShapeArgs.m_numCollisionShapes++; + return shapeIndex; } } + return -1; } -void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]) +int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -755,6 +758,112 @@ void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,doub command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[1] = halfExtents[1]; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[2] = halfExtents[2]; command->m_createCollisionShapeArgs.m_numCollisionShapes++; + return shapeIndex; + } + } + return -1; +} + +int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes; + if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CAPSULE; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height; + command->m_createCollisionShapeArgs.m_numCollisionShapes++; + return shapeIndex; + } + } + return -1; +} + +int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes; + if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CYLINDER; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height; + command->m_createCollisionShapeArgs.m_numCollisionShapes++; + return shapeIndex; + } + } + return -1; +} + + +int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[3], double planeConstant) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes; + if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_PLANE; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[0] = planeNormal[0]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[1] = planeNormal[1]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[2] = planeNormal[2]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeConstant = planeConstant; + command->m_createCollisionShapeArgs.m_numCollisionShapes++; + return shapeIndex; + } + } + return -1; +} + +int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes; + if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; + strcpy(command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshFileName,fileName); + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0; + command->m_createCollisionShapeArgs.m_numCollisionShapes++; + return shapeIndex; + } + } + return -1; +} + +void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + if (shapeIndexm_createCollisionShapeArgs.m_numCollisionShapes) + { + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags |= flags; } } } @@ -857,14 +966,14 @@ int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass int baseLinkIndex = numLinks; command->m_updateFlags |=MULTI_BODY_HAS_BASE; command->m_createMultiBodyArgs.m_baseLinkIndex = baseLinkIndex; - command->m_createMultiBodyArgs.m_baseWorldPosition[0]=basePosition[0]; - command->m_createMultiBodyArgs.m_baseWorldPosition[1]=basePosition[1]; - command->m_createMultiBodyArgs.m_baseWorldPosition[2]=basePosition[2]; + command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex*3+0]=basePosition[0]; + command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex*3+1]=basePosition[1]; + command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex*3+2]=basePosition[2]; - command->m_createMultiBodyArgs.m_baseWorldOrientation[0]=baseOrientation[0]; - command->m_createMultiBodyArgs.m_baseWorldOrientation[1]=baseOrientation[1]; - command->m_createMultiBodyArgs.m_baseWorldOrientation[2]=baseOrientation[2]; - command->m_createMultiBodyArgs.m_baseWorldOrientation[3]=baseOrientation[3]; + command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+0]=baseOrientation[0]; + command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+1]=baseOrientation[1]; + command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+2]=baseOrientation[2]; + command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+3]=baseOrientation[3]; command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+0] = mass; command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+1] = mass; @@ -883,7 +992,13 @@ int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass command->m_createMultiBodyArgs.m_linkCollisionShapeUniqueIds[baseLinkIndex]= collisionShapeUnique; command->m_createMultiBodyArgs.m_linkVisualShapeUniqueIds[baseLinkIndex] = visualShapeUniqueId; - command->m_createMultiBodyArgs.m_linkMasses[command->m_createMultiBodyArgs.m_numLinks] = mass; + command->m_createMultiBodyArgs.m_linkMasses[baseLinkIndex] = mass; + + command->m_createMultiBodyArgs.m_linkParentIndices[baseLinkIndex] = -2;//no parent + command->m_createMultiBodyArgs.m_linkJointAxis[baseLinkIndex+0]=0; + command->m_createMultiBodyArgs.m_linkJointAxis[baseLinkIndex+1]=0; + command->m_createMultiBodyArgs.m_linkJointAxis[baseLinkIndex+2]=0; + command->m_createMultiBodyArgs.m_linkJointTypes[baseLinkIndex]=-1; command->m_createMultiBodyArgs.m_numLinks++; } return numLinks; @@ -891,6 +1006,71 @@ int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass return -2; } +int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex, + double linkVisualShapeIndex, + double linkPosition[3], + double linkOrientation[4], + double linkInertialFramePosition[3], + double linkInertialFrameOrientation[4], + int linkParentIndex, + int linkJointType, + double linkJointAxis[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_MULTI_BODY); + if (command->m_type==CMD_CREATE_MULTI_BODY) + { + int numLinks = command->m_createMultiBodyArgs.m_numLinks; + + if (numLinksm_updateFlags |=MULTI_BODY_HAS_BASE; + command->m_createMultiBodyArgs.m_linkPositions[linkIndex*3+0]=linkPosition[0]; + command->m_createMultiBodyArgs.m_linkPositions[linkIndex*3+1]=linkPosition[1]; + command->m_createMultiBodyArgs.m_linkPositions[linkIndex*3+2]=linkPosition[2]; + + command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+0]=linkOrientation[0]; + command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+1]=linkOrientation[1]; + command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+2]=linkOrientation[2]; + command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+3]=linkOrientation[3]; + + command->m_createMultiBodyArgs.m_linkInertias[linkIndex*3+0] = linkMass; + command->m_createMultiBodyArgs.m_linkInertias[linkIndex*3+1] = linkMass; + command->m_createMultiBodyArgs.m_linkInertias[linkIndex*3+2] = linkMass; + + + command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex*3+0] = linkInertialFramePosition[0]; + command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex*3+1] = linkInertialFramePosition[1]; + command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex*3+2] = linkInertialFramePosition[2]; + + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+0] = linkInertialFrameOrientation[0]; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+1] = linkInertialFrameOrientation[1]; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+2] = linkInertialFrameOrientation[2]; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+3] = linkInertialFrameOrientation[3]; + + command->m_createMultiBodyArgs.m_linkCollisionShapeUniqueIds[linkIndex]= linkCollisionShapeIndex; + command->m_createMultiBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] = linkVisualShapeIndex; + + command->m_createMultiBodyArgs.m_linkParentIndices[linkIndex] = linkParentIndex; + command->m_createMultiBodyArgs.m_linkJointTypes[linkIndex] = linkJointType; + command->m_createMultiBodyArgs.m_linkJointAxis[3*linkIndex+0] = linkJointAxis[0]; + command->m_createMultiBodyArgs.m_linkJointAxis[3*linkIndex+1] = linkJointAxis[1]; + command->m_createMultiBodyArgs.m_linkJointAxis[3*linkIndex+2] = linkJointAxis[2]; + + command->m_createMultiBodyArgs.m_linkMasses[linkIndex] = linkMass; + command->m_createMultiBodyArgs.m_numLinks++; + return numLinks; + } + } + + return -1; +} + + + + //useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle) { @@ -3208,6 +3388,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..9b32a0daf 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -326,9 +326,13 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient); -void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius); -void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]); - +int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius); +int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]); +int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height); +int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height); +int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[3], double planeConstant); +int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[3]); +void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags); void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4]); @@ -340,6 +344,17 @@ int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient); int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4]); +int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex, + double linkVisualShapeIndex, + double linkPosition[3], + double linkOrientation[4], + double linkInertialFramePosition[3], + double linkInertialFrameOrientation[4], + int linkParentIndex, + int linkJointType, + double linkJointAxis[3]); + + //useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle); @@ -433,6 +448,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 6354e8fe0..d3a62ec52 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -11,8 +11,9 @@ #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" #include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyGearConstraint.h" - - +#include "../Importers/ImportURDFDemo/UrdfParser.h" +#include "../Utils/b3ResourcePath.h" +#include "Bullet3Common/b3FileUtils.h" #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" @@ -1153,7 +1154,8 @@ struct PhysicsServerCommandProcessorInternalData btAlignedObjectArray m_keyboardEvents; - + btAlignedObjectArray m_mouseEvents; + CommandLogger* m_commandLogger; CommandLogPlayback* m_logPlayback; @@ -1545,7 +1547,11 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range) virtual std::string getLinkName(int linkIndex) const { - return "link"; + std::string linkName = "link"; + char numstr[21]; // enough to hold all numbers up to 64-bits + sprintf(numstr, "%d", linkIndex); + linkName = linkName + numstr; + return linkName; } //various derived class in internal source code break with new pure virtual methods, so provide some default implementation @@ -1585,8 +1591,11 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface virtual std::string getJointName(int linkIndex) const { - b3Assert(0); - return "joint"; + std::string jointName = "joint"; + char numstr[21]; // enough to hold all numbers up to 64-bits + sprintf(numstr, "%d", linkIndex); + jointName = jointName + numstr; + return jointName; } //fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity. @@ -1619,6 +1628,14 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface ///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray& childLinkIndices) const { + for (int i=0;im_customVisualShapesConverter) + { + const UrdfModel& model = m_data->m_urdfParser.getModel(); + UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex); + if (linkPtr) + { + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId); + } + } + } + #endif } virtual void setBodyUniqueId(int bodyId) { @@ -2774,6 +2868,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"); @@ -3066,9 +3181,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, @@ -3451,6 +3586,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED; btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); + btCollisionShape* shape = 0; btCompoundShape* compound = 0; @@ -3505,6 +3641,121 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } break; } + case GEOM_CAPSULE: + { + shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius, + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + break; + } + case GEOM_CYLINDER: + { + shape = worldImporter->createCylinderShapeZ(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius, + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + break; + } + case GEOM_PLANE: + { + btVector3 planeNormal(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[0], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[1], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[2]); + + shape = worldImporter->createPlaneShape(planeNormal,0); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + break; + } + case GEOM_MESH: + { + btScalar defaultCollisionMargin = 0.001; + + btVector3 meshScale(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[0], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[1], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[2]); + + const std::string& urdf_path=""; + + std::string fileName = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshFileName; + + char relativeFileName[1024]; + char pathPrefix[1024]; + pathPrefix[0] = 0; + if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) + { + + b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + } + + const std::string& error_message_prefix=""; + std::string out_found_filename; + int out_type; + + bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); + if (foundFile) + { + if (out_type==UrdfGeometry::FILE_OBJ) + { + std::vector shapes; + std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str()); + //create a convex hull for each shape, and store it in a btCompoundShape + + //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); + //static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale) + B3_PROFILE("createConvexHullFromShapes"); + if (compound==0) + { + compound = worldImporter->createCompoundShape(); + } + compound->setMargin(defaultCollisionMargin); + + for (int s = 0; s<(int)shapes.size(); s++) + { + btConvexHullShape* convexHull = worldImporter->createConvexHullShape(); + convexHull->setMargin(defaultCollisionMargin); + tinyobj::shape_t& shape = shapes[s]; + int faceCount = shape.mesh.indices.size(); + + for (int f = 0; faddPoint(pt*meshScale,false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); + convexHull->addPoint(pt*meshScale, false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); + convexHull->addPoint(pt*meshScale, false); + } + + convexHull->recalcLocalAabb(); + convexHull->optimizeConvexHull(); + compound->addChildShape(childTransform,convexHull); + } + } + } + break; + } + default: + { + } } } #if 0 @@ -3551,6 +3802,33 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { m_data->m_sdfRecentLoadedBodies.clear(); + #if 0 + struct UrdfModel + { + std::string m_name; + std::string m_sourceFile; + btTransform m_rootTransformInWorld; + btHashMap m_materials; + btHashMap m_links; + btHashMap m_joints; + + btArray m_rootLinks; + bool m_overrideFixedBase; + + UrdfModel() + + clientCmd.m_createMultiBodyArgs. + + char m_bodyName[1024]; + int m_baseLinkIndex; + + double m_baseWorldPosition[3]; + double m_baseWorldOrientation[4]; + + UrdfModel tmpModel; + tmpModel.m_bodyName = + #endif + ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data); bool useMultiBody = true; @@ -7012,7 +7290,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++) @@ -7024,6 +7302,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..9658d6e6c 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 { @@ -811,10 +816,13 @@ struct b3CreateCollisionShape double m_capsuleFrom[3]; double m_capsuleTo[3]; double m_planeNormal[3]; + double m_planeConstant; int m_meshFileType; - char m_meshFileName[1024]; - double m_meshScale; + char m_meshFileName[VISUAL_SHAPE_MAX_PATH_LEN]; + double m_meshScale[3]; + int m_collisionFlags; + }; #define MAX_COMPOUND_COLLISION_SHAPES 16 @@ -842,17 +850,21 @@ struct b3CreateMultiBodyArgs char m_bodyName[1024]; int m_baseLinkIndex; - double m_baseWorldPosition[3]; - double m_baseWorldOrientation[4]; + double m_linkPositions[3*MAX_CREATE_MULTI_BODY_LINKS]; + double m_linkOrientations[4*MAX_CREATE_MULTI_BODY_LINKS]; int m_numLinks; double m_linkMasses[MAX_CREATE_MULTI_BODY_LINKS]; double m_linkInertias[MAX_CREATE_MULTI_BODY_LINKS*3]; + double m_linkInertialFramePositions[MAX_CREATE_MULTI_BODY_LINKS*3]; double m_linkInertialFrameOrientations[MAX_CREATE_MULTI_BODY_LINKS*4]; - int m_linkJointTypes[MAX_CREATE_MULTI_BODY_LINKS]; + int m_linkCollisionShapeUniqueIds[MAX_CREATE_MULTI_BODY_LINKS]; int m_linkVisualShapeUniqueIds[MAX_CREATE_MULTI_BODY_LINKS]; + int m_linkParentIndices[MAX_CREATE_MULTI_BODY_LINKS]; + int m_linkJointTypes[MAX_CREATE_MULTI_BODY_LINKS]; + double m_linkJointAxis[3*MAX_CREATE_MULTI_BODY_LINKS]; #if 0 std::string m_name; @@ -931,7 +943,6 @@ struct SharedMemoryCommand struct b3CreateVisualShapeArgs m_createVisualShapeArgs; struct b3CreateMultiBodyArgs m_createMultiBodyArgs; struct b3RequestCollisionInfoArgs m_requestCollisionInfoArgs; - }; }; @@ -1001,6 +1012,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..882db7a24 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 @@ -568,6 +594,12 @@ enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes GEOM_UNKNOWN, }; +enum eUrdfCollisionFlags +{ + GEOM_FORCE_CONCAVE_TRIMESH=1, +}; + + #endif//SHARED_MEMORY_PUBLIC_H diff --git a/examples/pybullet/examples/createMultiBodyLinks.py b/examples/pybullet/examples/createMultiBodyLinks.py new file mode 100644 index 000000000..7cfc03272 --- /dev/null +++ b/examples/pybullet/examples/createMultiBodyLinks.py @@ -0,0 +1,57 @@ +import pybullet as p +import time + + +p.connect(p.GUI) +p.createCollisionShape(p.GEOM_PLANE) +p.createMultiBody(0,0) + +sphereRadius = 0.05 +colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) +colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) + +mass = 1 +visualShapeId = -1 + +#"linkMasses","linkCollisionShapeIndices", "linkVisualShapeIndices", +#"linkPositions", "linkOrientations","linkParentIndices", "linkJointTypes","linkJointAxis", + +linkMasses=[1] +linkCollisionShapeIndices=[colBoxId] +linkVisualShapeIndices=[-1] +linkPositions=[[0,0,0.11]] +linkOrientations=[[0,0,0,1]] +linkInertialFramePositions=[[0,0,0]] +linkInertialFrameOrientations=[[0,0,0,1]] +indices=[0] +jointTypes=[p.JOINT_REVOLUTE] +axis=[[0,0,1]] + +for i in range (3): + for j in range (3): + for k in range (3): + basePosition = [1+i*5*sphereRadius,1+j*5*sphereRadius,1+k*5*sphereRadius+1] + baseOrientation = [0,0,0,1] + if (k&2): + sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,basePosition,baseOrientation) + else: + sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,basePosition,baseOrientation,linkMasses,linkCollisionShapeIndices,linkVisualShapeIndices,linkPositions,linkOrientations,linkInertialFramePositions, linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis) + + p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0) + for joint in range (p.getNumJoints(sphereUid)): + p.setJointMotorControl2(sphereUid,joint,p.VELOCITY_CONTROL,targetVelocity=1,force=10) + + +p.setGravity(0,0,-10) +#p.setRealTimeSimulation(1) + +p.getNumJoints(sphereUid) +for i in range (p.getNumJoints(sphereUid)): + p.getJointInfo(sphereUid,i) + +while (1): + events = p.getKeyboardEvents() + if (len(events)): + p.stepSimulation() + time.sleep(0.01) + \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index bbcc8c3cb..fdbf401a0 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -84,6 +84,24 @@ static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) return v; } +static int pybullet_internalGetIntFromSequence(PyObject* seq, int index) +{ + int v = 0; + PyObject* item; + + if (PyList_Check(seq)) + { + item = PyList_GET_ITEM(seq, index); + v = PyLong_AsLong(item); + } + else + { + item = PyTuple_GET_ITEM(seq, index); + v = PyLong_AsLong(item); + } + return v; +} + // internal function to set a float matrix[16] // used to initialize camera position with // a view and projection matrix in renderImage() @@ -197,6 +215,45 @@ static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) return 0; } + +static int pybullet_internalGetVector3FromSequence(PyObject* seq, int index, double vec[3]) +{ + int v = 0; + PyObject* item; + + if (PyList_Check(seq)) + { + item = PyList_GET_ITEM(seq, index); + pybullet_internalSetVectord(item,vec); + } + else + { + item = PyTuple_GET_ITEM(seq, index); + pybullet_internalSetVectord(item,vec); + } + return v; +} + +static int pybullet_internalGetVector4FromSequence(PyObject* seq, int index, double vec[4]) +{ + int v = 0; + PyObject* item; + + if (PyList_Check(seq)) + { + item = PyList_GET_ITEM(seq, index); + pybullet_internalSetVector4d(item,vec); + } + else + { + item = PyTuple_GET_ITEM(seq, index); + pybullet_internalSetVector4d(item,vec); + } + return v; +} + + + // Step through one timestep of the simulation static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObject* keywds) { @@ -3996,6 +4053,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; @@ -4847,12 +4948,21 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P int physicsClientId = 0; b3PhysicsClientHandle sm = 0; int shapeType=-1; - double radius=-1; + double radius=0.5; + double height = 1; + PyObject* meshScaleObj=0; + double meshScale[3] = {1,1,1}; + PyObject* planeNormalObj=0; + double planeNormal[3] = {0,0,1}; + + char* fileName=0; + int flags = 0; + PyObject* halfExtentsObj=0; - static char* kwlist[] = {"shapeType","radius","halfExtents", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOi", kwlist, - &shapeType, &radius,&halfExtentsObj, &physicsClientId)) + static char* kwlist[] = {"shapeType","radius","halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOii", kwlist, + &shapeType, &radius,&halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &physicsClientId)) { return NULL; } @@ -4866,16 +4976,41 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P { b3SharedMemoryStatusHandle statusHandle; int statusType; + int shapeIndex = -1; + b3SharedMemoryCommandHandle commandHandle = b3CreateCollisionShapeCommandInit(sm); if (shapeType==GEOM_SPHERE && radius>0) { - b3CreateCollisionShapeAddSphere(commandHandle,radius); + shapeIndex = b3CreateCollisionShapeAddSphere(commandHandle,radius); } if (shapeType==GEOM_BOX && halfExtentsObj) { double halfExtents[3] = {1,1,1}; pybullet_internalSetVectord(halfExtentsObj,halfExtents); - b3CreateCollisionShapeAddBox(commandHandle,halfExtents); + shapeIndex = b3CreateCollisionShapeAddBox(commandHandle,halfExtents); + } + if (shapeType==GEOM_CAPSULE && radius>0 && height>=0) + { + shapeIndex = b3CreateCollisionShapeAddCapsule(commandHandle,radius,height); + } + if (shapeType==GEOM_CYLINDER && radius>0 && height>=0) + { + shapeIndex = b3CreateCollisionShapeAddCylinder(commandHandle,radius,height); + } + if (shapeType==GEOM_MESH && fileName) + { + pybullet_internalSetVectord(meshScaleObj,meshScale); + shapeIndex = b3CreateCollisionShapeAddMesh(commandHandle, fileName,meshScale); + } + if (shapeType==GEOM_PLANE) + { + double planeConstant=0; + pybullet_internalSetVectord(planeNormalObj,planeNormal); + shapeIndex = b3CreateCollisionShapeAddPlane(commandHandle, planeNormal, planeConstant); + } + if (shapeIndex && flags) + { + b3CreateCollisionSetFlag(commandHandle,shapeIndex,flags); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); @@ -4949,14 +5084,30 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje int useMaximalCoordinates = 0; PyObject* basePosObj=0; PyObject* baseOrnObj=0; - + + PyObject* linkMassesObj=0; + PyObject* linkCollisionShapeIndicesObj=0; + PyObject* linkVisualShapeIndicesObj=0; + PyObject* linkPositionsObj=0; + PyObject* linkOrientationsObj=0; + PyObject* linkParentIndicesObj=0; + PyObject* linkJointTypesObj=0; + PyObject* linkJointAxisObj=0; + PyObject* linkInertialFramePositionObj=0; + PyObject* linkInertialFrameOrientationObj=0; static char* kwlist[] = {"baseMass","baseCollisionShapeIndex","baseVisualShapeIndex","basePosition", "baseOrientation", -// "linkParentIndices", "linkJointTypes","linkMasses","linkCollisionShapeIndices", -"useMaximalCoordinates","physicsClientId", + "linkMasses","linkCollisionShapeIndices", "linkVisualShapeIndices", + "linkPositions", "linkOrientations","linkInertialFramePosition","linkInertialFrameOrientation", "linkParentIndices", "linkJointTypes","linkJointAxis", + "useMaximalCoordinates","physicsClientId", + NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOii", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex, - &basePosObj, &baseOrnObj,&useMaximalCoordinates, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOOOOOOOOOOOii", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex, + &basePosObj, &baseOrnObj, + &linkMassesObj, &linkCollisionShapeIndicesObj, &linkVisualShapeIndicesObj, &linkPositionsObj, &linkOrientationsObj, + &linkInertialFramePositionObj, &linkInertialFrameOrientationObj, + &linkParentIndicesObj, &linkJointTypesObj,&linkJointAxisObj, + &useMaximalCoordinates, &physicsClientId)) { return NULL; } @@ -4968,27 +5119,167 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje } { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle commandHandle = b3CreateMultiBodyCommandInit(sm); - double basePosition[3]={0,0,0}; - double baseOrientation[4]={0,0,0,1}; - int baseIndex; - pybullet_internalSetVectord(basePosObj,basePosition); - pybullet_internalSetVector4d(baseOrnObj,baseOrientation); - baseIndex = b3CreateMultiBodyBase(commandHandle,baseMass,baseCollisionShapeIndex,baseVisualShapeIndex,basePosition,baseOrientation); - if (useMaximalCoordinates>0) + + + int numLinkMasses = linkMassesObj?PySequence_Size(linkMassesObj):0; + int numLinkCollisionShapes = linkCollisionShapeIndicesObj?PySequence_Size(linkCollisionShapeIndicesObj):0; + int numLinkVisualShapes = linkVisualShapeIndicesObj?PySequence_Size(linkVisualShapeIndicesObj):0; + int numLinkPositions = linkPositionsObj? PySequence_Size(linkPositionsObj):0; + int numLinkOrientations = linkOrientationsObj? PySequence_Size(linkOrientationsObj):0; + int numLinkParentIndices = linkParentIndicesObj?PySequence_Size(linkParentIndicesObj):0; + int numLinkJointTypes = linkJointTypesObj?PySequence_Size(linkJointTypesObj):0; + int numLinkJoinAxis = linkJointAxisObj? PySequence_Size(linkJointAxisObj):0; + int numLinkInertialFramePositions = linkInertialFramePositionObj? PySequence_Size(linkInertialFramePositionObj) : 0; + int numLinkInertialFrameOrientations = linkInertialFrameOrientationObj? PySequence_Size(linkInertialFrameOrientationObj) : 0; + + PyObject* seqLinkMasses = linkMassesObj?PySequence_Fast(linkMassesObj, "expected a sequence"):0; + PyObject* seqLinkCollisionShapes = linkCollisionShapeIndicesObj?PySequence_Fast(linkCollisionShapeIndicesObj, "expected a sequence"):0; + PyObject* seqLinkVisualShapes = linkVisualShapeIndicesObj?PySequence_Fast(linkVisualShapeIndicesObj, "expected a sequence"):0; + PyObject* seqLinkPositions = linkPositionsObj?PySequence_Fast(linkPositionsObj, "expected a sequence"):0; + PyObject* seqLinkOrientations = linkOrientationsObj?PySequence_Fast(linkOrientationsObj, "expected a sequence"):0; + PyObject* seqLinkParentIndices = linkParentIndicesObj?PySequence_Fast(linkParentIndicesObj, "expected a sequence"):0; + PyObject* seqLinkJointTypes = linkJointTypesObj?PySequence_Fast(linkJointTypesObj, "expected a sequence"):0; + PyObject* seqLinkJoinAxis = linkJointAxisObj?PySequence_Fast(linkJointAxisObj, "expected a sequence"):0; + PyObject* seqLinkInertialFramePositions = linkInertialFramePositionObj?PySequence_Fast(linkInertialFramePositionObj, "expected a sequence"):0; + PyObject* seqLinkInertialFrameOrientations = linkInertialFrameOrientationObj?PySequence_Fast(linkInertialFrameOrientationObj, "expected a sequence"):0; + + if ((numLinkMasses==numLinkCollisionShapes) && + (numLinkMasses==numLinkVisualShapes) && + (numLinkMasses==numLinkPositions) && + (numLinkMasses==numLinkOrientations) && + (numLinkMasses==numLinkParentIndices) && + (numLinkMasses==numLinkJointTypes) && + (numLinkMasses==numLinkJoinAxis) && + (numLinkMasses==numLinkInertialFramePositions) && + (numLinkMasses==numLinkInertialFrameOrientations)) { - b3CreateMultiBodyUseMaximalCoordinates(commandHandle); - } - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int i; + b3SharedMemoryCommandHandle commandHandle = b3CreateMultiBodyCommandInit(sm); + double basePosition[3]={0,0,0}; + double baseOrientation[4]={0,0,0,1}; + int baseIndex; + pybullet_internalSetVectord(basePosObj,basePosition); + pybullet_internalSetVector4d(baseOrnObj,baseOrientation); + baseIndex = b3CreateMultiBodyBase(commandHandle,baseMass,baseCollisionShapeIndex,baseVisualShapeIndex,basePosition,baseOrientation); + + for (i=0;i0) + { + b3CreateMultiBodyUseMaximalCoordinates(commandHandle); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) + { + int uid = b3GetStatusBodyIndex(statusHandle); + PyObject* ob = PyLong_FromLong(uid); + return ob; + } + + } else { - int uid = b3GetStatusBodyIndex(statusHandle); - PyObject* ob = PyLong_FromLong(uid); - return ob; + if (seqLinkMasses) + Py_DECREF(seqLinkMasses); + if (seqLinkCollisionShapes) + Py_DECREF(seqLinkCollisionShapes); + if (seqLinkVisualShapes) + Py_DECREF(seqLinkVisualShapes); + if (seqLinkPositions) + Py_DECREF(seqLinkPositions); + if (seqLinkOrientations) + Py_DECREF(seqLinkOrientations); + if (seqLinkParentIndices) + Py_DECREF(seqLinkParentIndices); + if (seqLinkJointTypes) + Py_DECREF(seqLinkJointTypes); + if (seqLinkJoinAxis) + Py_DECREF(seqLinkJoinAxis); + if (seqLinkInertialFramePositions) + Py_DECREF(seqLinkInertialFramePositions); + if (seqLinkInertialFrameOrientations) + Py_DECREF(seqLinkInertialFrameOrientations); + + PyErr_SetString(SpamError, "All link arrays need to be same size."); + return NULL; } + + + #if 0 + PyObject* seq; + seq = PySequence_Fast(objMat, "expected a sequence"); + if (seq) + { + len = PySequence_Size(objMat); + if (len == 16) + { + for (i = 0; i < len; i++) + { + matrix[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + } + #endif + + } PyErr_SetString(SpamError, "createMultiBody failed."); return NULL; @@ -6756,7 +7047,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 +7217,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); 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',