diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 523ec7471..5deb8d91c 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -155,6 +155,10 @@ struct b3PublicGraphicsInstanceData GLfloat m_color[4]; GLfloat m_scale[4]; + void clear() + { + } + }; typedef b3PoolBodyHandle b3PublicGraphicsInstance; diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp index 939587caf..564212f9d 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp @@ -28,6 +28,9 @@ B3_ATTRIBUTE_ALIGNED16(struct) SimpleGL2Instance b3Quaternion orn; b3Vector3 m_rgbColor; b3Vector3 m_scaling; + void clear() + { + } }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 94bbedc36..577e70e56 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1308,6 +1308,24 @@ b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHa command->m_userConstraintArguments.m_userConstraintUniqueId = userConstraintUniqueId; return (b3SharedMemoryCommandHandle)command; } + +b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_REMOVE_BODY; + command->m_updateFlags = BODY_DELETE_FLAG; + command->m_removeObjectArgs.m_numBodies = 1; + command->m_removeObjectArgs.m_bodyUniqueIds[0] = bodyUniqueId; + command->m_removeObjectArgs.m_numUserConstraints = 0; + + return (b3SharedMemoryCommandHandle)command; +} + int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index dcc210760..49f26d11a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -60,6 +60,8 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, ///If you re-connected to an existing server, or server changed otherwise, sync the body info and user constraints etc. b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient); +b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId); + ///return the total number of bodies in the simulation int b3GetNumBodies(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index fdd48a1b3..c64eb338e 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -190,6 +190,28 @@ PhysicsClientSharedMemory::~PhysicsClientSharedMemory() { delete m_data; } +void PhysicsClientSharedMemory::removeCachedBody(int bodyUniqueId) +{ + BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + + BodyJointInfoCache* bodyJoints = *bodyJointsPtr; + for (int j=0;jm_jointInfo.size();j++) + { + if (bodyJoints->m_jointInfo[j].m_jointName) + { + free(bodyJoints->m_jointInfo[j].m_jointName); + } + if (bodyJoints->m_jointInfo[j].m_linkName) + { + free(bodyJoints->m_jointInfo[j].m_linkName); + } + } + delete (*bodyJointsPtr); + m_data->m_bodyJointMap.remove(bodyUniqueId); + } +} void PhysicsClientSharedMemory::resetData() { m_data->m_debugLinesFrom.clear(); @@ -999,7 +1021,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { { break; } - + case CMD_REMOVE_BODY_COMPLETED: + { + break; + } + case CMD_REMOVE_BODY_FAILED: + { + b3Warning("Removing body failed"); + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); @@ -1050,7 +1080,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { return 0; } } - + + if (serverCmd.m_type == CMD_REMOVE_BODY_COMPLETED) + { + for (int i=0;im_userConstraintInfoMap.remove(cid); break; } + case CMD_REMOVE_BODY_FAILED: + { + b3Warning("Remove body failed\n"); + break; + } + case CMD_REMOVE_BODY_COMPLETED: + { + for (int i=0;im_bodyJointMap.size(); } +void PhysicsDirect::removeCachedBody(int bodyUniqueId) +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + for (int j=0;jm_jointInfo.size();j++) { + if (bodyJoints->m_jointInfo[j].m_jointName) + { + free(bodyJoints->m_jointInfo[j].m_jointName); + } + if (bodyJoints->m_jointInfo[j].m_linkName) + { + free(bodyJoints->m_jointInfo[j].m_linkName); + } + } + delete (*bodyJointsPtr); + m_data->m_bodyJointMap.remove(bodyUniqueId); + } +} + + int PhysicsDirect::getNumUserConstraints() const { return m_data->m_userConstraintInfoMap.size(); diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 8a9905de4..d3f71168a 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -29,6 +29,9 @@ protected: void postProcessStatus(const struct SharedMemoryStatus& serverCmd); void resetData(); + + void removeCachedBody(int bodyUniqueId); + public: PhysicsDirect(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index bef003805..36ce76be3 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -135,13 +135,21 @@ struct InteralBodyData btHashMap m_audioSources; #endif //B3_ENABLE_TINY_AUDIO - InteralBodyData() - :m_multiBody(0), - m_rigidBody(0), - m_testData(0) + InteralBodyData() { - m_rootLocalInertialFrame.setIdentity(); + clear(); } + + void clear() + { + m_multiBody=0; + m_rigidBody=0; + m_testData=0; + m_bodyName=""; + m_rootLocalInertialFrame.setIdentity(); + m_linkLocalInertialFrames.clear(); + } + }; struct InteralUserConstraintData @@ -1248,6 +1256,7 @@ struct PhysicsServerCommandProcessorInternalData { m_vrControllerEvents.init(); + m_bodyHandles.exitHandles(); m_bodyHandles.initHandles(); #if 0 btAlignedObjectArray bla; @@ -2849,7 +2858,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm ///this is a very rudimentary way to save the state of the world, for scene authoring ///many todo's, for example save the state of motor controllers etc. - { //saveWorld(clientCmd.m_sdfArguments.m_sdfFileName); @@ -4860,6 +4868,62 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = true; break; } + case CMD_REMOVE_BODY: + { + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_REMOVE_BODY_FAILED; + serverCmd.m_removeObjectArgs.m_numBodies = 0; + serverCmd.m_removeObjectArgs.m_numUserConstraints = 0; + + for (int i=0;im_bodyHandles.getHandle(bodyUniqueId); + if (bodyHandle) + { + if (bodyHandle->m_multiBody) + { + serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; + if (bodyHandle->m_multiBody->getBaseCollider()) + { + int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); + m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex); + m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getBaseCollider()); + } + for (int link=0;linkm_multiBody->getNumLinks();link++) + { + if (bodyHandle->m_multiBody->getLink(link).m_collider) + { + int graphicsIndex = bodyHandle->m_multiBody->getLink(link).m_collider->getUserIndex(); + m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex); + m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getLink(link).m_collider); + } + } + int numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects(); + m_data->m_dynamicsWorld->removeMultiBody(bodyHandle->m_multiBody); + numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects(); + //todo: clear all other remaining data, release memory etc + + serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; + } + if (bodyHandle->m_rigidBody) + { + serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; + //todo: clear all other remaining data... + m_data->m_dynamicsWorld->removeRigidBody(bodyHandle->m_rigidBody); + int graphicsInstance = bodyHandle->m_rigidBody->getUserIndex2(); + m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance); + serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; + } + } + + m_data->m_bodyHandles.freeHandle(bodyUniqueId); + } + + + hasStatus = true; + break; + } case CMD_USER_CONSTRAINT: { BT_PROFILE("CMD_USER_CONSTRAINT"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index f85a62d80..ecc302ac2 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -435,6 +435,14 @@ struct CreateBoxShapeArgs double m_colorRGBA[4]; }; +struct b3ObjectArgs +{ + int m_numBodies; + int m_bodyUniqueIds[MAX_SDF_BODIES]; + int m_numUserConstraints; + int m_userConstraintUniqueIds[MAX_SDF_BODIES]; +}; + struct SdfLoadedArgs { int m_numBodies; @@ -556,6 +564,10 @@ enum EnumUserConstraintFlags }; +enum EnumBodyChangeFlags +{ + BODY_DELETE_FLAG=1, +}; @@ -728,6 +740,8 @@ struct SharedMemoryCommand struct VRCameraState m_vrCameraStateArguments; struct StateLoggingRequest m_stateLoggingArguments; struct ConfigureOpenGLVisualizerRequest m_configureOpenGLVisualizerArguments; + struct b3ObjectArgs m_removeObjectArgs; + }; }; @@ -791,6 +805,7 @@ struct SharedMemoryStatus struct SendRaycastHits m_raycastHits; struct StateLoggingResultArgs m_stateLoggingResultArgs; struct b3OpenGLVisualizerCameraInfo m_visualizerCameraResultArgs; + struct b3ObjectArgs m_removeObjectArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index f54aef52c..5cb71729a 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -55,6 +55,7 @@ enum EnumSharedMemoryClientCommand CMD_CONFIGURE_OPENGL_VISUALIZER, CMD_REQUEST_KEYBOARD_EVENTS_DATA, CMD_REQUEST_OPENGL_VISUALIZER_CAMERA, + CMD_REMOVE_BODY, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -136,6 +137,8 @@ enum EnumSharedMemoryServerStatus CMD_REQUEST_KEYBOARD_EVENTS_DATA_FAILED, CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED, CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED, + CMD_REMOVE_BODY_COMPLETED, + CMD_REMOVE_BODY_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 8485d357b..6b1c0b884 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -714,7 +714,7 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key double startOrnY = 0.0; double startOrnZ = 0.0; double startOrnW = 1.0; - int useMaximalCoordinates = 0; + int useMaximalCoordinates = -1; int useFixedBase = 0; int backwardsCompatibilityArgs = 0; @@ -802,9 +802,9 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ); b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY, startOrnZ, startOrnW); - if (useMaximalCoordinates) + if (useMaximalCoordinates>=0) { - b3LoadUrdfCommandSetUseMultiBody(command, 0); + b3LoadUrdfCommandSetUseMultiBody(command, useMaximalCoordinates); } if (useFixedBase) { @@ -1695,6 +1695,40 @@ static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObje } } +static PyObject* pybullet_removeBody(PyObject* self, PyObject* args, PyObject* keywds) +{ + { + int bodyUniqueId = -1; + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + if (bodyUniqueId>=0) + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + if (b3CanSubmitCommand(sm)) + { + statusHandle = b3SubmitClientCommandAndWaitStatus( sm, b3InitRemoveBodyCommand(sm,bodyUniqueId)); + statusType = b3GetStatusType(statusHandle); + } + } + } + + Py_INCREF(Py_None); + return Py_None; +} + static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args, PyObject* keywds) { { @@ -5387,6 +5421,9 @@ static PyMethodDef SpamMethods[] = { {"getBodyInfo", (PyCFunction)pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS, "Get the body info, given a body unique id."}, + {"removeBody", (PyCFunction)pybullet_removeBody, METH_VARARGS | METH_KEYWORDS, + "Remove a body by its body unique id."}, + {"getNumConstraints", (PyCFunction)pybullet_getNumConstraints, METH_VARARGS | METH_KEYWORDS, "Get the number of user-created constraints in the simulation."}, diff --git a/src/Bullet3Common/b3ResizablePool.h b/src/Bullet3Common/b3ResizablePool.h index d5402517e..ff28c2218 100644 --- a/src/Bullet3Common/b3ResizablePool.h +++ b/src/Bullet3Common/b3ResizablePool.h @@ -16,11 +16,11 @@ struct b3PoolBodyHandle : public U B3_DECLARE_ALIGNED_ALLOCATOR(); int m_nextFreeHandle; - void SetNextFree(int next) + void setNextFree(int next) { m_nextFreeHandle = next; } - int GetNextFree() const + int getNextFree() const { return m_nextFreeHandle; } @@ -35,6 +35,16 @@ protected: int m_numUsedHandles; // number of active handles int m_firstFreeHandle; // free handles list + T* getHandleInternal(int handle) + { + return &m_bodyHandles[handle]; + + } + const T* getHandleInternal(int handle) const + { + return &m_bodyHandles[handle]; + } + public: b3ResizablePool() @@ -58,13 +68,15 @@ public: for (int i=0;i=0); @@ -73,11 +85,28 @@ public: { return 0; } - return &m_bodyHandles[handle]; + + if (m_bodyHandles[handle].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED) + { + return &m_bodyHandles[handle]; + } + return 0; + } const T* getHandle(int handle) const { - return &m_bodyHandles[handle]; + b3Assert(handle>=0); + b3Assert(handle=m_bodyHandles.size())) + { + return 0; + } + + if (m_bodyHandles[handle].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED) + { + return &m_bodyHandles[handle]; + } + return 0; } void increaseHandleCapacity(int extraCapacity) @@ -89,10 +118,10 @@ public: { for (int i = curCapacity; i < newCapacity; i++) - m_bodyHandles[i].SetNextFree(i + 1); + m_bodyHandles[i].setNextFree(i + 1); - m_bodyHandles[newCapacity - 1].SetNextFree(-1); + m_bodyHandles[newCapacity - 1].setNextFree(-1); } m_firstFreeHandle = curCapacity; } @@ -116,7 +145,7 @@ public: b3Assert(m_firstFreeHandle>=0); int handle = m_firstFreeHandle; - m_firstFreeHandle = getHandle(handle)->GetNextFree(); + m_firstFreeHandle = getHandleInternal(handle)->getNextFree(); m_numUsedHandles++; if (m_firstFreeHandle<0) @@ -126,10 +155,10 @@ public: increaseHandleCapacity(additionalCapacity); - getHandle(handle)->SetNextFree(m_firstFreeHandle); + getHandleInternal(handle)->setNextFree(m_firstFreeHandle); } - getHandle(handle)->SetNextFree(B3_POOL_HANDLE_TERMINAL_USED); - + getHandleInternal(handle)->setNextFree(B3_POOL_HANDLE_TERMINAL_USED); + getHandleInternal(handle)->clear(); return handle; } @@ -138,7 +167,8 @@ public: { b3Assert(handle >= 0); - getHandle(handle)->SetNextFree(m_firstFreeHandle); + getHandleInternal(handle)->clear(); + getHandleInternal(handle)->setNextFree(m_firstFreeHandle); m_firstFreeHandle = handle; m_numUsedHandles--;