Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests 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
Download Patch File Download Diff File Expand all files Collapse all files

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

43
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);
@@ -1050,7 +1080,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
return 0;
}
}
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

@@ -724,6 +724,20 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
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:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
@@ -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);

76
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -135,13 +135,21 @@ struct InteralBodyData
btHashMap<btHashInt, SDFAudioSource> 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<int> 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;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;
}
case CMD_USER_CONSTRAINT:
{
BT_PROFILE("CMD_USER_CONSTRAINT");

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