AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +00:00
Code Issues Releases Wiki Activity

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

Browse Source
This commit is contained in:
Erwin Coumans 2017-05-03 21:53:29 -07:00
parent 152e3da3e4
commit a86f584824
13 changed files with 278 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

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

3
examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp
View File

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

18
examples/SharedMemory/PhysicsClientC_API.cpp
View File

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

2
examples/SharedMemory/PhysicsClientC_API.h
View File

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

41
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

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

1
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

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

36
examples/SharedMemory/PhysicsDirect.cpp
View File

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

3
examples/SharedMemory/PhysicsDirect.h
View File

@ -29,6 +29,9 @@ protected:
void postProcessStatus(const struct SharedMemoryStatus& serverCmd);
void resetData();
void removeCachedBody(int bodyUniqueId);
public:
PhysicsDirect(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership);

74
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

15
examples/SharedMemory/SharedMemoryCommands.h
View File

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

3
examples/SharedMemory/SharedMemoryPublic.h
View File

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

43
examples/pybullet/pybullet.c
View File

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

50
src/Bullet3Common/b3ResizablePool.h
View File

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