more work towards removeBody for C-API/pybullet, work-in-progress.

examples/OpenGLWindow/GLInstancingRenderer.cpp

@@ -155,6 +155,10 @@ struct b3PublicGraphicsInstanceData
 	GLfloat m_color[4];
 	GLfloat m_scale[4];
 
+	void clear()
+	{
+	}
+
 };
 
 typedef b3PoolBodyHandle<b3PublicGraphicsInstanceData> b3PublicGraphicsInstance;

examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp

@@ -28,6 +28,9 @@ B3_ATTRIBUTE_ALIGNED16(struct) SimpleGL2Instance
 	b3Quaternion orn;
 	b3Vector3 m_rgbColor;
 	b3Vector3 m_scaling;
+	void clear()
+	{
+	}
 };
 
 

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;

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);
 

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;j<bodyJoints->m_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);
@@ -1051,6 +1081,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
             }
         }
 
+		if (serverCmd.m_type == CMD_REMOVE_BODY_COMPLETED)
+		{
+			for (int i=0;i<serverCmd.m_removeObjectArgs.m_numBodies;i++)
+			{
+				int bodyUniqueId = serverCmd.m_removeObjectArgs.m_bodyUniqueIds[i];
+				removeCachedBody(bodyUniqueId);
+			}
+		}
+
 		if (serverCmd.m_type == CMD_USER_CONSTRAINT_INFO_COMPLETED)
 		{
 			B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED");

examples/SharedMemory/PhysicsClientSharedMemory.h

@@ -13,6 +13,7 @@ protected:
 	virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
     void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
     void resetData();
+	void removeCachedBody(int bodyUniqueId);
 
 public:
     PhysicsClientSharedMemory();

examples/SharedMemory/PhysicsDirect.cpp

@@ -723,6 +723,20 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
         int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
         m_data->m_userConstraintInfoMap.remove(cid);
         break;
+    }
+	case CMD_REMOVE_BODY_FAILED:
+	{
+		b3Warning("Remove body failed\n");
+		break;
+	}
+	case CMD_REMOVE_BODY_COMPLETED:
+	{
+		for (int i=0;i<serverCmd.m_removeObjectArgs.m_numBodies;i++)
+		{
+			int bodyUniqueId = serverCmd.m_removeObjectArgs.m_bodyUniqueIds[i];
+			removeCachedBody(bodyUniqueId);
+		}
+		break;
 	}
 	case CMD_CHANGE_USER_CONSTRAINT_COMPLETED:
 	{
@@ -909,6 +923,28 @@ int PhysicsDirect::getNumBodies() const
 	return m_data->m_bodyJointMap.size();
 }
 
+void PhysicsDirect::removeCachedBody(int bodyUniqueId)
+{
+	BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
+	if (bodyJointsPtr && *bodyJointsPtr)
+	{
+		BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
+		for (int j=0;j<bodyJoints->m_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();

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);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -136,12 +136,20 @@ struct InteralBodyData
 #endif //B3_ENABLE_TINY_AUDIO
 
 	InteralBodyData()		
-		:m_multiBody(0),
-		m_rigidBody(0),
-		m_testData(0)
 	{
-		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<int> bla;
@@ -2850,7 +2859,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);
 						int constraintCount = 0;
@@ -4857,6 +4865,62 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 
                     SharedMemoryStatus& serverCmd =serverStatusOut;
                     serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
+                    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;i<clientCmd.m_removeObjectArgs.m_numBodies;i++)
+					{
+						int bodyUniqueId = clientCmd.m_removeObjectArgs.m_bodyUniqueIds[i];
+						InternalBodyHandle* bodyHandle = m_data->m_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;link<bodyHandle->m_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;
 				}

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;
 	};
 };
 

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
 };

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."},
 

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<m_bodyHandles.size();i++)
 		{
-			if (m_bodyHandles[i].GetNextFree()==B3_POOL_HANDLE_TERMINAL_USED)
+			if (m_bodyHandles[i].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED)
 			{
 				usedHandles.push_back(i);
 			}
 		}
 	}
 
+	
+
 	T* getHandle(int handle)
 	{
 		b3Assert(handle>=0);
@@ -73,12 +85,29 @@ public:
 		{
 			return 0;
 		}
+
+		if (m_bodyHandles[handle].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED)
+		{
 			return &m_bodyHandles[handle];
 		}
+		return 0;
+
+	}
 	const T* getHandle(int handle) const
+	{
+		b3Assert(handle>=0);
+		b3Assert(handle<m_bodyHandles.size());
+		if ((handle<0) || (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--;