Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Changes UserData to use global identifiers and makes linkIndex optional.

Browse Source 
This removes the need to specify the body id/link index when retrieving a user data entry.
Additionally, user data can now optionally be set to visual shapes as well.

The following public pybullet APIs have changed (backwards incompatible)
addUserData and getUserDataId
  Makes linkIndex parameter optional (default value is -1)
  Adds optional visualShapeIndex parameter (default value is -1)

getUserData and removeUserData
  Removes required parameters bodyUniqueId and linkIndex

getNumUserData
  Removes required bodyUniqueId parameter

getUserDataInfo
  Removes required linkIndex parameter
  Changes returned tuple from (userDataId, key) to (userDataId, key, bodyUniqueId, linkIndex, visualShapeIndex)
This commit is contained in:
Tigran Gasparian
2018-07-03 17:45:19 +02:00
parent 04556502f0
commit 9c7aa3a863
16 changed files with 478 additions and 555 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
examples/SharedMemory/PhysicsClient.h
View File

@@ -75,10 +75,10 @@ public:
virtual void setTimeOut(double timeOutInSeconds) = 0; virtual void setTimeOut(double timeOutInSeconds) = 0;
virtual double getTimeOut() const = 0; virtual double getTimeOut() const = 0;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const = 0; virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const = 0;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const = 0; virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const = 0;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const = 0; virtual int getNumUserData(int bodyUniqueId) const = 0;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const = 0; virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const = 0;
virtual void pushProfileTiming(const char* timingName)=0; virtual void pushProfileTiming(const char* timingName)=0;
virtual void popProfileTiming()=0; virtual void popProfileTiming()=0;

25
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2848,7 +2848,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsCl
return (b3SharedMemoryCommandHandle) command; return (b3SharedMemoryCommandHandle) command;
} }
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char* key, UserDataValueType valueType, int valueLength, const void *valueData) { B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, UserDataValueType valueType, int valueLength, const void *valueData) {
PhysicsClient* cl = (PhysicsClient* ) physClient; PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(strlen(key) < MAX_USER_DATA_KEY_LENGTH); b3Assert(strlen(key) < MAX_USER_DATA_KEY_LENGTH);
b3Assert(cl); b3Assert(cl);
@@ -2860,6 +2860,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsCli
command->m_type = CMD_ADD_USER_DATA; command->m_type = CMD_ADD_USER_DATA;
command->m_addUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId; command->m_addUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId;
command->m_addUserDataRequestArgs.m_linkIndex = linkIndex; command->m_addUserDataRequestArgs.m_linkIndex = linkIndex;
command->m_addUserDataRequestArgs.m_visualShapeIndex = visualShapeIndex;
command->m_addUserDataRequestArgs.m_valueType = valueType; command->m_addUserDataRequestArgs.m_valueType = valueType;
command->m_addUserDataRequestArgs.m_valueLength = valueLength; command->m_addUserDataRequestArgs.m_valueLength = valueLength;
strcpy(command->m_addUserDataRequestArgs.m_key, key); strcpy(command->m_addUserDataRequestArgs.m_key, key);
@@ -2868,7 +2869,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsCli
return (b3SharedMemoryCommandHandle) command; return (b3SharedMemoryCommandHandle) command;
} }
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId) { B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int userDataId) {
PhysicsClient* cl = (PhysicsClient* ) physClient; PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl); b3Assert(cl);
b3Assert(cl->canSubmitCommand()); b3Assert(cl->canSubmitCommand());
@@ -2876,29 +2877,27 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3Physics
b3Assert(command); b3Assert(command);
command->m_type = CMD_REMOVE_USER_DATA; command->m_type = CMD_REMOVE_USER_DATA;
command->m_removeUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId;
command->m_removeUserDataRequestArgs.m_linkIndex = linkIndex;
command->m_removeUserDataRequestArgs.m_userDataId = userDataId; command->m_removeUserDataRequestArgs.m_userDataId = userDataId;
return (b3SharedMemoryCommandHandle) command; return (b3SharedMemoryCommandHandle) command;
} }
B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue *valueOut) B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int userDataId, struct b3UserDataValue *valueOut)
{ {
PhysicsClient* cl = (PhysicsClient*)physClient; PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl) if (cl)
{ {
return cl->getCachedUserData(bodyUniqueId, linkIndex, userDataId, *valueOut); return cl->getCachedUserData(userDataId, *valueOut);
} }
return false; return false;
} }
B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char *key) B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key)
{ {
PhysicsClient* cl = (PhysicsClient*)physClient; PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl) if (cl)
{ {
return cl->getCachedUserDataId(bodyUniqueId, linkIndex, key); return cl->getCachedUserDataId(bodyUniqueId, linkIndex, visualShapeIndex, key);
} }
return -1; return -1;
} }
@@ -2909,27 +2908,27 @@ B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHan
if (status) if (status)
{ {
btAssert(status->m_type == CMD_ADD_USER_DATA_COMPLETED); btAssert(status->m_type == CMD_ADD_USER_DATA_COMPLETED);
return status->m_userDataResponseArgs.m_userDataGlobalId.m_userDataId; return status->m_userDataResponseArgs.m_userDataId;
} }
return -1; return -1;
} }
B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex) B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId)
{ {
PhysicsClient* cl = (PhysicsClient*)physClient; PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl) if (cl)
{ {
return cl->getNumUserData(bodyUniqueId, linkIndex); return cl->getNumUserData(bodyUniqueId);
} }
return 0; return 0;
} }
B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut)
{ {
PhysicsClient* cl = (PhysicsClient*)physClient; PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl) if (cl)
{ {
cl->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut); cl->getUserDataInfo(bodyUniqueId, userDataIndex, keyOut, userDataIdOut, linkIndexOut, visualShapeIndexOut);
} }
} }

12
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -115,14 +115,14 @@ B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqu
///user data handling ///user data handling
B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsClientHandle physClient); B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char* key, enum UserDataValueType valueType, int valueLength, const void *valueData); B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, enum UserDataValueType valueType, int valueLength, const void *valueData);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId); B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int userDataId);
B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue *valueOut); B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int userDataId, struct b3UserDataValue *valueOut);
B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char *key); B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key);
B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHandle); B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut); B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut);
B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h ///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h

153
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -15,26 +15,12 @@
#include "LinearMath/btQuickprof.h" #include "LinearMath/btQuickprof.h"
struct UserDataCache
{
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<btHashString, int> m_keyToUserDataIdMap;
UserDataCache()
{
}
~UserDataCache()
{
}
};
struct BodyJointInfoCache struct BodyJointInfoCache
{ {
std::string m_baseName; std::string m_baseName;
b3AlignedObjectArray<b3JointInfo> m_jointInfo; b3AlignedObjectArray<b3JointInfo> m_jointInfo;
std::string m_bodyName; std::string m_bodyName;
// Joint index -> user data. btAlignedObjectArray<int> m_userDataIds;
btHashMap<btHashInt, UserDataCache> m_jointToUserDataMap;
~BodyJointInfoCache() ~BodyJointInfoCache()
{ {
@@ -78,7 +64,10 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<int> m_bodyIdsRequestInfo; btAlignedObjectArray<int> m_bodyIdsRequestInfo;
btAlignedObjectArray<int> m_constraintIdsRequestInfo; btAlignedObjectArray<int> m_constraintIdsRequestInfo;
btAlignedObjectArray<b3UserDataGlobalIdentifier> m_userDataIdsRequestInfo; btAlignedObjectArray<int> m_userDataIdsRequestInfo;
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;
SharedMemoryStatus m_tempBackupServerStatus; SharedMemoryStatus m_tempBackupServerStatus;
@@ -243,6 +232,12 @@ void PhysicsClientSharedMemory::removeCachedBody(int bodyUniqueId)
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) if (bodyJointsPtr && *bodyJointsPtr)
{ {
for(int i=0; i<(*bodyJointsPtr)->m_userDataIds.size(); i++) {
const int userDataId = (*bodyJointsPtr)->m_userDataIds[i];
SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataMap.remove(userDataId);
}
delete (*bodyJointsPtr); delete (*bodyJointsPtr);
m_data->m_bodyJointMap.remove(bodyUniqueId); m_data->m_bodyJointMap.remove(bodyUniqueId);
} }
@@ -264,6 +259,8 @@ void PhysicsClientSharedMemory::resetData()
} }
m_data->m_bodyJointMap.clear(); m_data->m_bodyJointMap.clear();
m_data->m_userConstraintInfoMap.clear(); m_data->m_userConstraintInfoMap.clear();
m_data->m_userDataHandleLookup.clear();
m_data->m_userDataMap.clear();
} }
void PhysicsClientSharedMemory::setSharedMemoryKey(int key) void PhysicsClientSharedMemory::setSharedMemoryKey(int key)
@@ -1409,25 +1406,26 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr) if (bodyJointsPtr && *bodyJointsPtr)
{ {
(*bodyJointsPtr)->m_jointToUserDataMap.clear(); (*bodyJointsPtr)->m_userDataIds.clear();
} }
m_data->m_userDataMap.clear();
m_data->m_userDataHandleLookup.clear();
} }
const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers; const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers;
if (numIdentifiers > 0) { if (numIdentifiers > 0) {
m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus; m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;
const b3UserDataGlobalIdentifier *identifiers = (b3UserDataGlobalIdentifier *)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; const int *identifiers = (int *)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i=0; i<numIdentifiers; i++) { m_data->m_userDataIdsRequestInfo.reserve(numIdentifiers - 1);
// Store the identifiers that still need to be requested.
for (int i=0; i<numIdentifiers - 1; i++) {
m_data->m_userDataIdsRequestInfo.push_back(identifiers[i]); m_data->m_userDataIdsRequestInfo.push_back(identifiers[i]);
} }
// Request individual user data entries. // Request individual user data entries, start with last identifier.
const b3UserDataGlobalIdentifier userDataGlobalId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1];
m_data->m_userDataIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
command.m_type = CMD_REQUEST_USER_DATA; command.m_type = CMD_REQUEST_USER_DATA;
command.m_userDataRequestArgs = userDataGlobalId; command.m_userDataRequestArgs.m_userDataId = identifiers[numIdentifiers - 1];
submitClientCommand(command); submitClientCommand(command);
return 0; return 0;
} }
@@ -1435,31 +1433,23 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (serverCmd.m_type == CMD_ADD_USER_DATA_COMPLETED || serverCmd.m_type == CMD_REQUEST_USER_DATA_COMPLETED) { if (serverCmd.m_type == CMD_ADD_USER_DATA_COMPLETED || serverCmd.m_type == CMD_REQUEST_USER_DATA_COMPLETED) {
B3_PROFILE("CMD_ADD_USER_DATA_COMPLETED"); B3_PROFILE("CMD_ADD_USER_DATA_COMPLETED");
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId; const UserDataResponseArgs response = serverCmd.m_userDataResponseArgs;
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[response.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) { if (bodyJointsPtr && *bodyJointsPtr) {
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
if (!userDataCachePtr)
{
UserDataCache cache;
(*bodyJointsPtr)->m_jointToUserDataMap.insert(userDataGlobalId.m_linkIndex, cache);
}
userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
const char *dataStream = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; const char *dataStream = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
SharedMemoryUserData* userData = m_data->m_userDataMap[response.m_userDataId];
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; if (userData) {
if (userDataPtr) {
// Only replace the value. // Only replace the value.
userDataPtr->replaceValue(dataStream, serverCmd.m_userDataResponseArgs.m_valueLength, serverCmd.m_userDataResponseArgs.m_valueType); userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType);
} }
else { else {
// Add a new user data entry. // Add a new user data entry.
const char *key = serverCmd.m_userDataResponseArgs.m_key; const char *key = response.m_key;
(userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(key)); m_data->m_userDataMap.insert(response.m_userDataId, SharedMemoryUserData(key, response.m_bodyUniqueId, response.m_linkIndex, response.m_visualShapeIndex));
(userDataCachePtr)->m_keyToUserDataIdMap.insert(key, userDataGlobalId.m_userDataId); userData = m_data->m_userDataMap[response.m_userDataId];
userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType);
userDataPtr->replaceValue(dataStream, serverCmd.m_userDataResponseArgs.m_valueLength, serverCmd.m_userDataResponseArgs.m_valueType); m_data->m_userDataHandleLookup.insert(SharedMemoryUserDataHashKey(userData), response.m_userDataId);
(*bodyJointsPtr)->m_userDataIds.push_back(response.m_userDataId);
} }
} }
} }
@@ -1468,12 +1458,12 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (m_data->m_userDataIdsRequestInfo.size() > 0) { if (m_data->m_userDataIdsRequestInfo.size() > 0) {
// Request individual user data entries. // Request individual user data entries.
const b3UserDataGlobalIdentifier userDataGlobalId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1]; const int userDataId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1];
m_data->m_userDataIdsRequestInfo.pop_back(); m_data->m_userDataIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
command.m_type = CMD_REQUEST_USER_DATA; command.m_type = CMD_REQUEST_USER_DATA;
command.m_userDataRequestArgs = userDataGlobalId; command.m_userDataRequestArgs.m_userDataId = userDataId;
submitClientCommand(command); submitClientCommand(command);
return 0; return 0;
} }
@@ -1482,19 +1472,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (serverCmd.m_type == CMD_REMOVE_USER_DATA_COMPLETED) { if (serverCmd.m_type == CMD_REMOVE_USER_DATA_COMPLETED) {
B3_PROFILE("CMD_REMOVE_USER_DATA_COMPLETED"); B3_PROFILE("CMD_REMOVE_USER_DATA_COMPLETED");
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs; const int userDataId = serverCmd.m_removeUserDataResponseArgs.m_userDataId;
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
if (userData) {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userData->m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) { if (bodyJointsPtr && *bodyJointsPtr) {
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; (*bodyJointsPtr)->m_userDataIds.remove(userDataId);
if (userDataCachePtr)
{
SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr)
{
(userDataCachePtr)->m_keyToUserDataIdMap.remove((userDataPtr)->m_key.c_str());
(userDataCachePtr)->m_userDataMap.remove(userDataGlobalId.m_userDataId);
}
} }
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataMap.remove(userDataId);
} }
} }
@@ -1852,16 +1838,8 @@ double PhysicsClientSharedMemory::getTimeOut() const
return m_data->m_timeOutInSeconds; return m_data->m_timeOutInSeconds;
} }
bool PhysicsClientSharedMemory::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const { bool PhysicsClientSharedMemory::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; SharedMemoryUserData *userDataPtr = m_data->m_userDataMap[userDataId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return false;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr) {
return false;
}
SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId];
if (!userDataPtr) if (!userDataPtr)
{ {
return false; return false;
@@ -1872,54 +1850,37 @@ bool PhysicsClientSharedMemory::getCachedUserData(int bodyUniqueId, int linkInde
return true; return true;
} }
int PhysicsClientSharedMemory::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { int PhysicsClientSharedMemory::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; int* userDataId = m_data->m_userDataHandleLookup.find(SharedMemoryUserDataHashKey(key, bodyUniqueId, linkIndex, visualShapeIndex));
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return -1;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr)
{
return -1;
}
int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key];
if (!userDataId) { if (!userDataId) {
return -1; return -1;
} }
return *userDataId; return *userDataId;
} }
int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId, int linkIndex) const { int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) { if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return 0; return 0;
} }
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; return (*bodyJointsPtr)->m_userDataIds.size();
if (!userDataCachePtr)
{
return 0;
}
return (userDataCachePtr)->m_userDataMap.size();
} }
void PhysicsClientSharedMemory::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const { void PhysicsClientSharedMemory::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) if (!bodyJointsPtr || !(*bodyJointsPtr) || userDataIndex < 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size())
{ {
*keyOut = 0; *keyOut = 0;
*userDataIdOut = -1; *userDataIdOut = -1;
return; return;
} }
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; int userDataId = (*bodyJointsPtr)->m_userDataIds[userDataIndex];
if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size()) SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
{
*keyOut = 0; *userDataIdOut = userDataId;
*userDataIdOut = -1; *keyOut = userData->m_key.c_str();
return; *linkIndexOut = userData->m_linkIndex;
} *visualShapeIndexOut = userData->m_visualShapeIndex;
*userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1();
SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex);
*keyOut = (userDataPtr)->m_key.c_str();
} }

8
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -85,10 +85,10 @@ public:
virtual void setTimeOut(double timeOutInSeconds); virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const; virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const; virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const; virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const; virtual int getNumUserData(int bodyUniqueId) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const; virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const;
virtual void pushProfileTiming(const char* timingName); virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming(); virtual void popProfileTiming();

139
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -16,24 +16,13 @@
#include "SharedMemoryUserData.h" #include "SharedMemoryUserData.h"
#include "LinearMath/btQuickprof.h" #include "LinearMath/btQuickprof.h"
struct UserDataCache {
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<btHashString, int> m_keyToUserDataIdMap;
~UserDataCache()
{
}
};
struct BodyJointInfoCache2 struct BodyJointInfoCache2
{ {
std::string m_baseName; std::string m_baseName;
btAlignedObjectArray<b3JointInfo> m_jointInfo; btAlignedObjectArray<b3JointInfo> m_jointInfo;
std::string m_bodyName; std::string m_bodyName;
btAlignedObjectArray<int> m_userDataIds;
// Joint index -> user data.
btHashMap<btHashInt, UserDataCache> m_jointToUserDataMap;
~BodyJointInfoCache2() { ~BodyJointInfoCache2() {
} }
@@ -85,6 +74,9 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<b3RayHitInfo> m_raycastHits; btAlignedObjectArray<b3RayHitInfo> m_raycastHits;
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;
PhysicsCommandProcessorInterface* m_commandProcessor; PhysicsCommandProcessorInterface* m_commandProcessor;
bool m_ownsCommandProcessor; bool m_ownsCommandProcessor;
double m_timeOutInSeconds; double m_timeOutInSeconds;
@@ -685,34 +677,23 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
} }
void PhysicsDirect::processAddUserData(const struct SharedMemoryStatus& serverCmd) { void PhysicsDirect::processAddUserData(const struct SharedMemoryStatus& serverCmd) {
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId; const UserDataResponseArgs response = serverCmd.m_userDataResponseArgs;
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[response.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) if (bodyJointsPtr && *bodyJointsPtr) {
{
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
if (!userDataCachePtr)
{
UserDataCache cache;
(*bodyJointsPtr)->m_jointToUserDataMap.insert(userDataGlobalId.m_linkIndex, cache);
}
userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
const char *dataStream = m_data->m_bulletStreamDataServerToClient; const char *dataStream = m_data->m_bulletStreamDataServerToClient;
SharedMemoryUserData* userData = m_data->m_userDataMap[response.m_userDataId];
b3UserDataValue userDataValue; if (userData) {
userDataValue.m_type = serverCmd.m_userDataResponseArgs.m_valueType;
userDataValue.m_length = serverCmd.m_userDataResponseArgs.m_valueLength;
SharedMemoryUserData *userDataPtr = userDataCachePtr->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr) {
// Only replace the value. // Only replace the value.
(userDataPtr)->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type); userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType);
} }
else { else {
// Add a new user data entry. // Add a new user data entry.
(userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(serverCmd.m_userDataResponseArgs.m_key)); const char *key = response.m_key;
userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; m_data->m_userDataMap.insert(response.m_userDataId, SharedMemoryUserData(key, response.m_bodyUniqueId, response.m_linkIndex, response.m_visualShapeIndex));
userDataPtr->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type); userData = m_data->m_userDataMap[response.m_userDataId];
(userDataCachePtr)->m_keyToUserDataIdMap.insert(serverCmd.m_userDataResponseArgs.m_key, userDataGlobalId.m_userDataId); userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType);
m_data->m_userDataHandleLookup.insert(SharedMemoryUserDataHashKey(userData), response.m_userDataId);
(*bodyJointsPtr)->m_userDataIds.push_back(response.m_userDataId);
} }
} }
} }
@@ -1152,16 +1133,18 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr) if (bodyJointsPtr && *bodyJointsPtr)
{ {
(*bodyJointsPtr)->m_jointToUserDataMap.clear(); (*bodyJointsPtr)->m_userDataIds.clear();
} }
m_data->m_userDataMap.clear();
m_data->m_userDataHandleLookup.clear();
} }
const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers; const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers;
b3UserDataGlobalIdentifier *identifiers = new b3UserDataGlobalIdentifier[numIdentifiers]; int *identifiers = new int[numIdentifiers];
memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(b3UserDataGlobalIdentifier)); memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(int));
for (int i=0; i<numIdentifiers; i++) { for (int i=0; i<numIdentifiers; i++) {
m_data->m_tmpInfoRequestCommand.m_type = CMD_REQUEST_USER_DATA; m_data->m_tmpInfoRequestCommand.m_type = CMD_REQUEST_USER_DATA;
m_data->m_tmpInfoRequestCommand.m_userDataRequestArgs = identifiers[i]; m_data->m_tmpInfoRequestCommand.m_userDataRequestArgs.m_userDataId = identifiers[i];
bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
@@ -1184,18 +1167,15 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
} }
case CMD_REMOVE_USER_DATA_COMPLETED: case CMD_REMOVE_USER_DATA_COMPLETED:
{ {
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs; const int userDataId = serverCmd.m_removeUserDataResponseArgs.m_userDataId;
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
if (userData) {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userData->m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) { if (bodyJointsPtr && *bodyJointsPtr) {
UserDataCache *userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; (*bodyJointsPtr)->m_userDataIds.remove(userDataId);
if (userDataCachePtr)
{
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr) {
(userDataCachePtr)->m_keyToUserDataIdMap.remove((userDataPtr)->m_key.c_str());
(userDataCachePtr)->m_userDataMap.remove(userDataGlobalId.m_userDataId);
}
} }
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataMap.remove(userDataId);
} }
break; break;
} }
@@ -1253,6 +1233,12 @@ void PhysicsDirect::removeCachedBody(int bodyUniqueId)
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) if (bodyJointsPtr && *bodyJointsPtr)
{ {
for(int i=0; i<(*bodyJointsPtr)->m_userDataIds.size(); i++) {
const int userDataId = (*bodyJointsPtr)->m_userDataIds[i];
SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataMap.remove(userDataId);
}
delete (*bodyJointsPtr); delete (*bodyJointsPtr);
m_data->m_bodyJointMap.remove(bodyUniqueId); m_data->m_bodyJointMap.remove(bodyUniqueId);
} }
@@ -1499,72 +1485,49 @@ double PhysicsDirect::getTimeOut() const
return m_data->m_timeOutInSeconds; return m_data->m_timeOutInSeconds;
} }
bool PhysicsDirect::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const { bool PhysicsDirect::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; SharedMemoryUserData *userDataPtr = m_data->m_userDataMap[userDataId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return false;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr)
{
return false;
}
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId];
if (!userDataPtr) if (!userDataPtr)
{ {
return false; return false;
} }
valueOut.m_type = userDataPtr->m_type; valueOut.m_type = (userDataPtr)->m_type;
valueOut.m_length = userDataPtr->m_bytes.size(); valueOut.m_length = userDataPtr->m_bytes.size();
valueOut.m_data1 = userDataPtr->m_bytes.size()? &userDataPtr->m_bytes[0] : 0; valueOut.m_data1 = userDataPtr->m_bytes.size()? &userDataPtr->m_bytes[0] : 0;
return true; return true;
} }
int PhysicsDirect::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { int PhysicsDirect::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; int* userDataId = m_data->m_userDataHandleLookup.find(SharedMemoryUserDataHashKey(key, bodyUniqueId, linkIndex, visualShapeIndex));
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return -1;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr) {
return -1;
}
int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key];
if (!userDataId) { if (!userDataId) {
return -1; return -1;
} }
return *userDataId; return *userDataId;
} }
int PhysicsDirect::getNumUserData(int bodyUniqueId, int linkIndex) const { int PhysicsDirect::getNumUserData(int bodyUniqueId) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) { if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return 0; return 0;
} }
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; return (*bodyJointsPtr)->m_userDataIds.size();
if (!userDataCachePtr) {
return 0;
}
return (userDataCachePtr)->m_userDataMap.size();
} }
void PhysicsDirect::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const { void PhysicsDirect::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) { if (!bodyJointsPtr || !(*bodyJointsPtr) || userDataIndex <= 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size())
*keyOut = 0;
*userDataIdOut = -1;
return;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size())
{ {
*keyOut = 0; *keyOut = 0;
*userDataIdOut = -1; *userDataIdOut = -1;
return; return;
} }
*userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1(); int userDataId = (*bodyJointsPtr)->m_userDataIds[userDataIndex];
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex); SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
*keyOut = (userDataPtr)->m_key.c_str();
*userDataIdOut = userDataId;
*keyOut = userData->m_key.c_str();
*linkIndexOut = userData->m_linkIndex;
*visualShapeIndexOut = userData->m_visualShapeIndex;
} }

8
examples/SharedMemory/PhysicsDirect.h
View File

@@ -115,10 +115,10 @@ public:
virtual void setTimeOut(double timeOutInSeconds); virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const; virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const; virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const; virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const; virtual int getNumUserData(int bodyUniqueId) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const; virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const;
virtual void pushProfileTiming(const char* timingName); virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming(); virtual void popProfileTiming();

16
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -231,20 +231,20 @@ double PhysicsLoopBack::getTimeOut() const
return m_data->m_physicsClient->getTimeOut(); return m_data->m_physicsClient->getTimeOut();
} }
bool PhysicsLoopBack::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const { bool PhysicsLoopBack::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const {
return m_data->m_physicsClient->getCachedUserData(bodyUniqueId, linkIndex, userDataId, valueOut); return m_data->m_physicsClient->getCachedUserData(userDataId, valueOut);
} }
int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const {
return m_data->m_physicsClient->getCachedUserDataId(bodyUniqueId, linkIndex, key); return m_data->m_physicsClient->getCachedUserDataId(bodyUniqueId, linkIndex, visualShapeIndex, key);
} }
int PhysicsLoopBack::getNumUserData(int bodyUniqueId, int linkIndex) const { int PhysicsLoopBack::getNumUserData(int bodyUniqueId) const {
return m_data->m_physicsClient->getNumUserData(bodyUniqueId, linkIndex); return m_data->m_physicsClient->getNumUserData(bodyUniqueId);
} }
void PhysicsLoopBack::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const { void PhysicsLoopBack::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const {
m_data->m_physicsClient->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut); m_data->m_physicsClient->getUserDataInfo(bodyUniqueId, userDataIndex, keyOut, userDataIdOut, linkIndexOut, visualShapeIndexOut);
} }
void PhysicsLoopBack::pushProfileTiming(const char* timingName) void PhysicsLoopBack::pushProfileTiming(const char* timingName)

8
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -89,10 +89,10 @@ public:
virtual void setTimeOut(double timeOutInSeconds); virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const; virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const; virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const; virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const; virtual int getNumUserData(int bodyUniqueId) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const; virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const;
virtual void pushProfileTiming(const char* timingName); virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming(); virtual void popProfileTiming();

193
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -222,72 +222,6 @@ struct InternalCollisionShapeData
#include "SharedMemoryUserData.h" #include "SharedMemoryUserData.h"
/**
* Holds all custom user data entries for a link.
*/
struct InternalLinkUserData {
// Used to look up user data entry handles for string keys.
btHashMap<btHashString, int> m_keyToHandleMap;
b3ResizablePool<b3PoolBodyHandle<SharedMemoryUserData> > m_userData;
// Adds or replaces a user data entry.
// Returns the user data handle.
const int add(const char* key, const char* bytes, int len, int type)
{
// If an entry with the key already exists, just update the value.
int userDataId = getUserDataId(key);
if (userDataId != -1) {
SharedMemoryUserData* userData = m_userData.getHandle(userDataId);
b3Assert(userData);
userData->replaceValue(bytes, len, type);
return userDataId;
}
userDataId = m_userData.allocHandle();
SharedMemoryUserData* userData = m_userData.getHandle(userDataId);
userData->m_key = key;
userData->replaceValue(bytes, len, type);
m_keyToHandleMap.insert(userData->m_key.c_str(), userDataId);
return userDataId;
}
// Returns the user data handle for a specified key or -1 if not found.
const int getUserDataId(const char* key) const
{
const int* userDataIdPtr = m_keyToHandleMap.find(key);
if (userDataIdPtr)
{
return *userDataIdPtr;
}
return -1;
}
// Removes a user data entry given the handle.
// Returns true when the entry was removed, false otherwise.
const bool remove(int userDataId)
{
const SharedMemoryUserData* userData = m_userData.getHandle(userDataId);
if (!userData)
{
return false;
}
m_keyToHandleMap.remove(userData->m_key.c_str());
m_userData.freeHandle(userDataId);
return true;
}
// Returns the user data given the user data id. null otherwise.
const SharedMemoryUserData* getUserData(const int userDataId) const
{
return m_userData.getHandle(userDataId);
}
void getUserDataIds(b3AlignedObjectArray<int> &userDataIds) const
{
m_userData.getUsedHandles(userDataIds);
}
};
struct InternalBodyData struct InternalBodyData
{ {
btMultiBody* m_multiBody; btMultiBody* m_multiBody;
@@ -303,7 +237,7 @@ struct InternalBodyData
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_rigidBodyJoints; btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_rigidBodyJoints;
btAlignedObjectArray<std::string> m_rigidBodyJointNames; btAlignedObjectArray<std::string> m_rigidBodyJointNames;
btAlignedObjectArray<std::string> m_rigidBodyLinkNames; btAlignedObjectArray<std::string> m_rigidBodyLinkNames;
btHashMap<btHashInt, InternalLinkUserData*> m_linkUserDataMap; btAlignedObjectArray<int> m_userDataHandles;
#ifdef B3_ENABLE_TINY_AUDIO #ifdef B3_ENABLE_TINY_AUDIO
b3HashMap<btHashInt, SDFAudioSource> m_audioSources; b3HashMap<btHashInt, SDFAudioSource> m_audioSources;
@@ -328,10 +262,7 @@ struct InternalBodyData
m_rigidBodyJoints.clear(); m_rigidBodyJoints.clear();
m_rigidBodyJointNames.clear(); m_rigidBodyJointNames.clear();
m_rigidBodyLinkNames.clear(); m_rigidBodyLinkNames.clear();
for(int i=0; i<m_linkUserDataMap.size(); i++) { m_userDataHandles.clear();
delete *m_linkUserDataMap.getAtIndex(i);
}
m_linkUserDataMap.clear();
} }
}; };
@@ -1607,8 +1538,8 @@ struct PhysicsServerCommandProcessorInternalData
b3ResizablePool< InternalBodyHandle > m_bodyHandles; b3ResizablePool< InternalBodyHandle > m_bodyHandles;
b3ResizablePool<InternalCollisionShapeHandle> m_userCollisionShapeHandles; b3ResizablePool<InternalCollisionShapeHandle> m_userCollisionShapeHandles;
b3ResizablePool<InternalVisualShapeHandle> m_userVisualShapeHandles; b3ResizablePool<InternalVisualShapeHandle> m_userVisualShapeHandles;
b3ResizablePool<b3PoolBodyHandle<SharedMemoryUserData>> m_userDataHandles;
btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;
b3PluginManager m_pluginManager; b3PluginManager m_pluginManager;
@@ -5042,34 +4973,11 @@ bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct Shar
bool hasStatus = true; bool hasStatus = true;
BT_PROFILE("CMD_SYNC_USER_DATA"); BT_PROFILE("CMD_SYNC_USER_DATA");
b3UserDataGlobalIdentifier *userDataIdentifiers = (b3UserDataGlobalIdentifier *)bufferServerToClient; b3AlignedObjectArray<int> userDataHandles;
int numIdentifiers = 0; m_data->m_userDataHandles.getUsedHandles(userDataHandles);
b3AlignedObjectArray<int> bodyHandles; memcpy(bufferServerToClient, &userDataHandles[0], sizeof(int) * userDataHandles.size());
m_data->m_bodyHandles.getUsedHandles(bodyHandles);
for (int i=0; i<bodyHandles.size(); i++) {
int bodyHandle = bodyHandles[i];
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyHandle);
if (!body) {
continue;
}
for (int j=0; j<body->m_linkUserDataMap.size(); j++) {
const int linkIndex = body->m_linkUserDataMap.getKeyAtIndex(j).getUid1();
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap.getAtIndex(j);
if (!userDataPtr) {
continue;
}
b3AlignedObjectArray<int> userDataIds;
(*userDataPtr)->getUserDataIds(userDataIds);
for (int k=0; k<userDataIds.size(); k++) {
b3UserDataGlobalIdentifier &identifier = userDataIdentifiers[numIdentifiers++];
identifier.m_bodyUniqueId = bodyHandle;
identifier.m_linkIndex = linkIndex;
identifier.m_userDataId = userDataIds[k];
}
}
}
serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = numIdentifiers; serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = userDataHandles.size();
serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED; serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED;
return hasStatus; return hasStatus;
} }
@@ -5080,21 +4988,16 @@ bool PhysicsServerCommandProcessor::processRequestUserDataCommand(const struct S
BT_PROFILE("CMD_REQUEST_USER_DATA"); BT_PROFILE("CMD_REQUEST_USER_DATA");
serverStatusOut.m_type = CMD_REQUEST_USER_DATA_FAILED; serverStatusOut.m_type = CMD_REQUEST_USER_DATA_FAILED;
InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_userDataRequestArgs.m_bodyUniqueId); SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(clientCmd.m_userDataRequestArgs.m_userDataId);
if (!body) {
return hasStatus;
}
const int linkIndex = clientCmd.m_userDataRequestArgs.m_linkIndex;
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex];
if (!userDataPtr) {
return hasStatus;
}
const SharedMemoryUserData *userData = (*userDataPtr)->getUserData(clientCmd.m_userDataRequestArgs.m_userDataId);
if (!userData) { if (!userData) {
return hasStatus; return hasStatus;
} }
btAssert(bufferSizeInBytes >= userData->m_bytes.size()); btAssert(bufferSizeInBytes >= userData->m_bytes.size());
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId = clientCmd.m_userDataRequestArgs; serverStatusOut.m_userDataResponseArgs.m_userDataId = clientCmd.m_userDataRequestArgs.m_userDataId;
serverStatusOut.m_userDataResponseArgs.m_bodyUniqueId = userData->m_bodyUniqueId;
serverStatusOut.m_userDataResponseArgs.m_linkIndex = userData->m_linkIndex;
serverStatusOut.m_userDataResponseArgs.m_visualShapeIndex = userData->m_visualShapeIndex;
serverStatusOut.m_userDataResponseArgs.m_valueType = userData->m_type; serverStatusOut.m_userDataResponseArgs.m_valueType = userData->m_type;
serverStatusOut.m_userDataResponseArgs.m_valueLength = userData->m_bytes.size(); serverStatusOut.m_userDataResponseArgs.m_valueLength = userData->m_bytes.size();
serverStatusOut.m_type = CMD_REQUEST_USER_DATA_COMPLETED; serverStatusOut.m_type = CMD_REQUEST_USER_DATA_COMPLETED;
@@ -5122,22 +5025,38 @@ bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct Share
if (!body) { if (!body) {
return hasStatus; return hasStatus;
} }
const int linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex;
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex]; SharedMemoryUserDataHashKey userDataIdentifier(
if (!userDataPtr) { clientCmd.m_addUserDataRequestArgs.m_key,
InternalLinkUserData *userData = new InternalLinkUserData; clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId,
userDataPtr = &userData; clientCmd.m_addUserDataRequestArgs.m_linkIndex,
body->m_linkUserDataMap.insert(linkIndex, userData); clientCmd.m_addUserDataRequestArgs.m_visualShapeIndex);
int* userDataHandlePtr = m_data->m_userDataHandleLookup.find(userDataIdentifier);
int userDataHandle = userDataHandlePtr ? *userDataHandlePtr : m_data->m_userDataHandles.allocHandle();
SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(userDataHandle);
if (!userData) {
return hasStatus;
} }
const int userDataId = (*userDataPtr)->add(clientCmd.m_addUserDataRequestArgs.m_key, if (!userDataHandlePtr) {
bufferServerToClient,clientCmd.m_addUserDataRequestArgs.m_valueLength, userData->m_key = clientCmd.m_addUserDataRequestArgs.m_key;
userData->m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId;
userData->m_linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex;
userData->m_visualShapeIndex = clientCmd.m_addUserDataRequestArgs.m_visualShapeIndex;
m_data->m_userDataHandleLookup.insert(userDataIdentifier, userDataHandle);
body->m_userDataHandles.push_back(userDataHandle);
}
userData->replaceValue(bufferServerToClient,
clientCmd.m_addUserDataRequestArgs.m_valueLength,
clientCmd.m_addUserDataRequestArgs.m_valueType); clientCmd.m_addUserDataRequestArgs.m_valueType);
serverStatusOut.m_type = CMD_ADD_USER_DATA_COMPLETED; serverStatusOut.m_type = CMD_ADD_USER_DATA_COMPLETED;
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_userDataId = userDataId; serverStatusOut.m_userDataResponseArgs.m_userDataId = userDataHandle;
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId; serverStatusOut.m_userDataResponseArgs.m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId;
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex; serverStatusOut.m_userDataResponseArgs.m_linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex;
serverStatusOut.m_userDataResponseArgs.m_visualShapeIndex = clientCmd.m_addUserDataRequestArgs.m_visualShapeIndex;
serverStatusOut.m_userDataResponseArgs.m_valueLength = clientCmd.m_addUserDataRequestArgs.m_valueLength; serverStatusOut.m_userDataResponseArgs.m_valueLength = clientCmd.m_addUserDataRequestArgs.m_valueLength;
serverStatusOut.m_userDataResponseArgs.m_valueType = clientCmd.m_addUserDataRequestArgs.m_valueType; serverStatusOut.m_userDataResponseArgs.m_valueType = clientCmd.m_addUserDataRequestArgs.m_valueType;
strcpy(serverStatusOut.m_userDataResponseArgs.m_key, clientCmd.m_addUserDataRequestArgs.m_key); strcpy(serverStatusOut.m_userDataResponseArgs.m_key, clientCmd.m_addUserDataRequestArgs.m_key);
@@ -5152,19 +5071,20 @@ bool PhysicsServerCommandProcessor::processRemoveUserDataCommand(const struct Sh
BT_PROFILE("CMD_REMOVE_USER_DATA"); BT_PROFILE("CMD_REMOVE_USER_DATA");
serverStatusOut.m_type = CMD_REMOVE_USER_DATA_FAILED; serverStatusOut.m_type = CMD_REMOVE_USER_DATA_FAILED;
InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_removeUserDataRequestArgs.m_bodyUniqueId); SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
if (!userData) {
return hasStatus;
}
InternalBodyData *body = m_data->m_bodyHandles.getHandle(userData->m_bodyUniqueId);
if (!body) { if (!body) {
return hasStatus; return hasStatus;
} }
const int linkIndex = clientCmd.m_removeUserDataRequestArgs.m_linkIndex; body->m_userDataHandles.remove(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex];
if (!userDataPtr) { m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
return hasStatus; m_data->m_userDataHandles.freeHandle(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
}
const bool removed = (*userDataPtr)->remove(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
if (!removed) {
return hasStatus;
}
serverStatusOut.m_removeUserDataResponseArgs = clientCmd.m_removeUserDataRequestArgs; serverStatusOut.m_removeUserDataResponseArgs = clientCmd.m_removeUserDataRequestArgs;
serverStatusOut.m_type = CMD_REMOVE_USER_DATA_COMPLETED; serverStatusOut.m_type = CMD_REMOVE_USER_DATA_COMPLETED;
return hasStatus; return hasStatus;
@@ -8278,6 +8198,12 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
bodyHandle->m_rigidBody=0; bodyHandle->m_rigidBody=0;
serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
} }
for (int i=0; i < bodyHandle->m_userDataHandles.size(); i++) {
int userDataHandle = bodyHandle->m_userDataHandles[i];
SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(userDataHandle);
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataHandles.freeHandle(userDataHandle);
}
m_data->m_bodyHandles.freeHandle(bodyUniqueId); m_data->m_bodyHandles.freeHandle(bodyUniqueId);
} }
@@ -10495,6 +10421,9 @@ void PhysicsServerCommandProcessor::resetSimulation()
m_data->m_userCollisionShapeHandles.exitHandles(); m_data->m_userCollisionShapeHandles.exitHandles();
m_data->m_userCollisionShapeHandles.initHandles(); m_data->m_userCollisionShapeHandles.initHandles();
m_data->m_userDataHandles.exitHandles();
m_data->m_userDataHandles.initHandles();
m_data->m_userDataHandleLookup.clear();
} }

18
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -993,13 +993,20 @@ struct b3StateSerializationArguments
struct SyncUserDataArgs struct SyncUserDataArgs
{ {
// User data identifiers stored in m_bulletStreamDataServerToClientRefactor // User data identifiers stored in m_bulletStreamDataServerToClientRefactor
// as as array of b3UserDataGlobalIdentifier objects // as as array of integers.
int m_numUserDataIdentifiers; int m_numUserDataIdentifiers;
}; };
struct UserDataRequestArgs {
int m_userDataId;
};
struct UserDataResponseArgs struct UserDataResponseArgs
{ {
b3UserDataGlobalIdentifier m_userDataGlobalId; int m_userDataId;
int m_bodyUniqueId;
int m_linkIndex;
int m_visualShapeIndex;
int m_valueType; int m_valueType;
int m_valueLength; int m_valueLength;
char m_key[MAX_USER_DATA_KEY_LENGTH]; char m_key[MAX_USER_DATA_KEY_LENGTH];
@@ -1010,6 +1017,7 @@ struct AddUserDataRequestArgs
{ {
int m_bodyUniqueId; int m_bodyUniqueId;
int m_linkIndex; int m_linkIndex;
int m_visualShapeIndex;
int m_valueType; int m_valueType;
int m_valueLength; int m_valueLength;
char m_key[MAX_USER_DATA_KEY_LENGTH]; char m_key[MAX_USER_DATA_KEY_LENGTH];
@@ -1073,9 +1081,9 @@ struct SharedMemoryCommand
struct b3CustomCommand m_customCommandArgs; struct b3CustomCommand m_customCommandArgs;
struct b3StateSerializationArguments m_loadStateArguments; struct b3StateSerializationArguments m_loadStateArguments;
struct RequestCollisionShapeDataArgs m_requestCollisionShapeDataArguments; struct RequestCollisionShapeDataArgs m_requestCollisionShapeDataArguments;
struct b3UserDataGlobalIdentifier m_userDataRequestArgs; struct UserDataRequestArgs m_userDataRequestArgs;
struct AddUserDataRequestArgs m_addUserDataRequestArgs; struct AddUserDataRequestArgs m_addUserDataRequestArgs;
struct b3UserDataGlobalIdentifier m_removeUserDataRequestArgs; struct UserDataRequestArgs m_removeUserDataRequestArgs;
}; };
}; };
@@ -1150,7 +1158,7 @@ struct SharedMemoryStatus
struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs; struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs;
struct SyncUserDataArgs m_syncUserDataArgs; struct SyncUserDataArgs m_syncUserDataArgs;
struct UserDataResponseArgs m_userDataResponseArgs; struct UserDataResponseArgs m_userDataResponseArgs;
struct b3UserDataGlobalIdentifier m_removeUserDataResponseArgs; struct UserDataRequestArgs m_removeUserDataResponseArgs;
}; };
}; };

8
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -274,6 +274,7 @@ struct b3JointInfo
enum UserDataValueType { enum UserDataValueType {
USER_DATA_VALUE_TYPE_NOT_USET = -1,
// Data represents generic byte array. // Data represents generic byte array.
USER_DATA_VALUE_TYPE_BYTES = 0, USER_DATA_VALUE_TYPE_BYTES = 0,
// Data represents C-string // Data represents C-string
@@ -287,13 +288,6 @@ struct b3UserDataValue
char* m_data1; char* m_data1;
}; };
struct b3UserDataGlobalIdentifier
{
int m_bodyUniqueId;
int m_linkIndex;
int m_userDataId;
};
struct b3UserConstraint struct b3UserConstraint
{ {
int m_parentBodyIndex; int m_parentBodyIndex;

53
examples/SharedMemory/SharedMemoryUserData.h
View File

@@ -3,6 +3,7 @@
#include <string> #include <string>
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btHashMap.h"
#include "SharedMemoryPublic.h" #include "SharedMemoryPublic.h"
struct SharedMemoryUserData struct SharedMemoryUserData
@@ -10,20 +11,22 @@ struct SharedMemoryUserData
std::string m_key; std::string m_key;
int m_type; int m_type;
int m_bodyUniqueId;
int m_linkIndex;
int m_visualShapeIndex;
btAlignedObjectArray<char> m_bytes; btAlignedObjectArray<char> m_bytes;
SharedMemoryUserData() SharedMemoryUserData()
:m_type(-1) :m_type(-1), m_bodyUniqueId(-1), m_linkIndex(-1), m_visualShapeIndex(-1)
{ {
} }
// Takes ownership of the passed key and value arguments. SharedMemoryUserData(const char* key, int bodyUniqueId, int linkIndex, int visualShapeIndex)
SharedMemoryUserData(const char* key) :m_key(key), m_type(-1), m_bodyUniqueId(bodyUniqueId), m_linkIndex(linkIndex), m_visualShapeIndex(visualShapeIndex)
:m_key(key)
{ {
} }
// Takes ownership of the data pointed to by newValue.
void replaceValue(const char* bytes, int len, int type) void replaceValue(const char* bytes, int len, int type)
{ {
m_type = type; m_type = type;
@@ -45,4 +48,44 @@ struct SharedMemoryUserData
} }
}; };
class SharedMemoryUserDataHashKey {
unsigned int m_hash = 0;
btHashString m_key;
btHashInt m_bodyUniqueId;
btHashInt m_linkIndex;
btHashInt m_visualShapeIndex;
public:
SIMD_FORCE_INLINE unsigned int getHash()const {
return m_hash;
}
SharedMemoryUserDataHashKey() : m_hash(0) {}
SharedMemoryUserDataHashKey(const struct SharedMemoryUserData *userData)
: m_key(userData->m_key.c_str()),
m_bodyUniqueId(userData->m_bodyUniqueId),
m_linkIndex(userData->m_linkIndex),
m_visualShapeIndex(userData->m_visualShapeIndex) {
calculateHash();
}
SharedMemoryUserDataHashKey(const char *key, int bodyUniqueId, int linkIndex, int visualShapeIndex)
: m_key(key), m_bodyUniqueId(bodyUniqueId), m_linkIndex(linkIndex), m_visualShapeIndex(visualShapeIndex) {
calculateHash();
}
void calculateHash() {
m_hash = m_key.getHash() ^ m_bodyUniqueId.getHash() ^ m_linkIndex.getHash() ^ m_visualShapeIndex.getHash();
}
bool equals(const SharedMemoryUserDataHashKey& other) const {
return m_bodyUniqueId.equals(other.m_bodyUniqueId) &&
m_linkIndex.equals(other.m_linkIndex) &&
m_visualShapeIndex.equals(other.m_visualShapeIndex) &&
m_key.equals(other.m_key);
}
};
#endif //SHARED_MEMORY_USER_DATA_H #endif //SHARED_MEMORY_USER_DATA_H

66
examples/pybullet/examples/userData.py
View File

@@ -21,16 +21,16 @@ plane_id = client.loadURDF(PLANE_PATH)
print ("Plane ID: %s" % plane_id) print ("Plane ID: %s" % plane_id)
print ("Adding user data to plane") print ("Adding user data to plane")
MyKey1 = client.addUserData(plane_id, 0, "MyKey1", "MyValue1") MyKey1 = client.addUserData(plane_id, "MyKey1", "MyValue1")
MyKey2 = client.addUserData(plane_id, 0, "MyKey2", "MyValue2") MyKey2 = client.addUserData(plane_id, "MyKey2", "MyValue2")
MyKey3 = client.addUserData(plane_id, 0, "MyKey3", "MyValue3") MyKey3 = client.addUserData(plane_id, "MyKey3", "MyValue3")
MyKey4 = client.addUserData(plane_id, 0, "MyKey4", "MyValue4") MyKey4 = client.addUserData(plane_id, "MyKey4", "MyValue4")
print ("Retrieving cached user data") print ("Retrieving cached user data")
print (client.getUserData(plane_id, 0, MyKey1)) print (client.getUserData(MyKey1))
print (client.getUserData(plane_id, 0, MyKey2)) print (client.getUserData(MyKey2))
print (client.getUserData(plane_id, 0, MyKey3)) print (client.getUserData(MyKey3))
print (client.getUserData(plane_id, 0, MyKey4)) print (client.getUserData(MyKey4))
print ("Disconnecting") print ("Disconnecting")
del client del client
@@ -39,18 +39,18 @@ print ("Reconnecting")
client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD) client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD)
print ("Retrieving synced user data") print ("Retrieving synced user data")
print (client.getUserData(plane_id, 0, MyKey1)) print (client.getUserData(MyKey1))
print (client.getUserData(plane_id, 0, MyKey2)) print (client.getUserData(MyKey2))
print (client.getUserData(plane_id, 0, MyKey3)) print (client.getUserData(MyKey3))
print (client.getUserData(plane_id, 0, MyKey4)) print (client.getUserData(MyKey4))
print ("Number of user data entries: %s" % client.getNumUserData(plane_id, 0)) print ("Number of user data entries: %s" % client.getNumUserData(plane_id))
print ("Overriding user data") print ("Overriding user data")
client.addUserData(plane_id, 0, "MyKey1", "MyNewValue") client.addUserData(plane_id, "MyKey1", "MyNewValue")
print ("Cached overridden data") print ("Cached overridden data")
print (client.getUserData(plane_id, 0, MyKey1)) print (client.getUserData(MyKey1))
print ("Disconnecting") print ("Disconnecting")
@@ -61,50 +61,50 @@ client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD)
print ("Synced overridden data") print ("Synced overridden data")
print (client.getUserData(plane_id, 0, MyKey1)) print (client.getUserData(MyKey1))
print ("Getting user data ID") print ("Getting user data ID")
print ("Retrieved ID: %s, ID retrieved from addUserData: %s" % (client.getUserDataId(plane_id, 0, "MyKey2"), MyKey2)) print ("Retrieved ID: %s, ID retrieved from addUserData: %s" % (client.getUserDataId(plane_id, "MyKey2"), MyKey2))
print ("Removing user data") print ("Removing user data")
client.removeUserData(plane_id, 0, MyKey2) client.removeUserData(MyKey2)
print ("Retrieving cached removed data") print ("Retrieving cached removed data")
print (client.getUserData(plane_id, 0, MyKey2)) print (client.getUserData(MyKey2))
print ("Syncing") print ("Syncing")
client.syncUserData() client.syncUserData()
print ("Retrieving removed removed data") print ("Retrieving removed removed data")
print (client.getUserData(plane_id, 0, MyKey2)) print (client.getUserData(MyKey2))
print ("Iterating over all user data entries and printing results") print ("Iterating over all user data entries and printing results")
for i in range(client.getNumUserData(plane_id, 0)): for i in range(client.getNumUserData(plane_id)):
userDataId, key = client.getUserDataInfo(plane_id, 0, i) userDataId, key, bodyId, linkIndex, visualShapeIndex = client.getUserDataInfo(plane_id, i)
print ("Info: (%s, %s)" % (userDataId, key)) print ("Info: (%s, %s, %s, %s, %s)" % (userDataId, key, bodyId, linkIndex, visualShapeIndex))
print ("Value: %s" % client.getUserData(plane_id, 0, userDataId)) print ("Value: %s" % client.getUserData(userDataId))
print ("Removing body") print ("Removing body")
client.removeBody(plane_id) client.removeBody(plane_id)
print ("Retrieving user data") print ("Retrieving user data")
print (client.getUserData(plane_id, 0, MyKey1)) print (client.getUserData(MyKey1))
print (client.getUserData(plane_id, 0, MyKey3)) print (client.getUserData(MyKey3))
print (client.getUserData(plane_id, 0, MyKey4)) print (client.getUserData(MyKey4))
print ("Syncing") print ("Syncing")
client.syncUserData() client.syncUserData()
print ("Retrieving user data") print ("Retrieving user data")
print (client.getUserData(plane_id, 0, MyKey1)) print (client.getUserData(MyKey1))
print (client.getUserData(plane_id, 0, MyKey3)) print (client.getUserData(MyKey3))
print (client.getUserData(plane_id, 0, MyKey4)) print (client.getUserData(MyKey4))
plane_id2 = client.loadURDF(PLANE_PATH) plane_id2 = client.loadURDF(PLANE_PATH)
print ("Plane1: %s, plane2: %s" % (plane_id, plane_id2)) print ("Plane1: %s, plane2: %s" % (plane_id, plane_id2))
print ("Retrieving user data") print ("Retrieving user data")
print (client.getUserData(plane_id, 0, MyKey1)) print (client.getUserData(MyKey1))
print (client.getUserData(plane_id, 0, MyKey3)) print (client.getUserData(MyKey3))
print (client.getUserData(plane_id, 0, MyKey4)) print (client.getUserData(MyKey4))

73
examples/pybullet/pybullet.c
View File

@@ -689,18 +689,19 @@ static PyObject* pybullet_addUserData(PyObject* self, PyObject* args, PyObject*
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
int physicsClientId = 0; int physicsClientId = 0;
int bodyUniqueId = -1; int bodyUniqueId = -1;
int linkIndex = -2; int linkIndex = -1;
int visualShapeIndex = -1;
const char* key = ""; const char* key = "";
const char* value = ""; // TODO: Change this to a PyObject and detect the type dynamically. const char* value = ""; // TODO: Change this to a PyObject and detect the type dynamically.
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "key", "value", "physicsClientId", NULL}; static char* kwlist[] = {"bodyUniqueId", "key", "value", "linkIndex", "visualShapeIndex", "physicsClientId", NULL};
b3SharedMemoryCommandHandle command; b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int statusType; int statusType;
int userDataId; int userDataId;
int valueLen=-1; int valueLen=-1;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiss|i", kwlist, &bodyUniqueId, &linkIndex, &key, &value, &physicsClientId)) if (!PyArg_ParseTupleAndKeywords(args, keywds, "iss|iii", kwlist, &bodyUniqueId, &key, &value, &linkIndex, &visualShapeIndex, &physicsClientId))
{ {
return NULL; return NULL;
} }
@@ -712,7 +713,7 @@ static PyObject* pybullet_addUserData(PyObject* self, PyObject* args, PyObject*
} }
valueLen = strlen(value)+1; valueLen = strlen(value)+1;
command = b3InitAddUserDataCommand(sm, bodyUniqueId, linkIndex, key, USER_DATA_VALUE_TYPE_STRING, valueLen, value); command = b3InitAddUserDataCommand(sm, bodyUniqueId, linkIndex, visualShapeIndex, key, USER_DATA_VALUE_TYPE_STRING, valueLen, value);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle); statusType = b3GetStatusType(statusHandle);
@@ -730,16 +731,14 @@ static PyObject* pybullet_removeUserData(PyObject* self, PyObject* args, PyObjec
{ {
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
int physicsClientId = 0; int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
int userDataId = -1; int userDataId = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataId", "physicsClientId", NULL}; static char* kwlist[] = {"userDataId", "physicsClientId", NULL};
b3SharedMemoryCommandHandle command; b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int statusType; int statusType;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataId, &physicsClientId)) if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &userDataId, &physicsClientId))
{ {
return NULL; return NULL;
} }
@@ -750,7 +749,7 @@ static PyObject* pybullet_removeUserData(PyObject* self, PyObject* args, PyObjec
return NULL; return NULL;
} }
command = b3InitRemoveUserDataCommand(sm, bodyUniqueId, linkIndex, userDataId); command = b3InitRemoveUserDataCommand(sm, userDataId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle); statusType = b3GetStatusType(statusHandle);
@@ -770,15 +769,16 @@ static PyObject* pybullet_getUserDataId(PyObject* self, PyObject* args, PyObject
int physicsClientId = 0; int physicsClientId = 0;
int bodyUniqueId = -1; int bodyUniqueId = -1;
int linkIndex = -1; int linkIndex = -1;
int visualShapeIndex = -1;
const char* key = ""; const char* key = "";
int userDataId; int userDataId;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "key", "physicsClientId", NULL}; static char* kwlist[] = {"bodyUniqueId", "key", "linkIndex", "visualShapeIndex", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iis|i", kwlist, &bodyUniqueId, &linkIndex, &key, &physicsClientId)) if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|iii", kwlist, &bodyUniqueId, &key, &linkIndex, &visualShapeIndex, &physicsClientId))
{ {
return NULL; return NULL;
} }
@@ -789,7 +789,7 @@ static PyObject* pybullet_getUserDataId(PyObject* self, PyObject* args, PyObject
return NULL; return NULL;
} }
userDataId = b3GetUserDataId(sm, bodyUniqueId, linkIndex, key); userDataId = b3GetUserDataId(sm, bodyUniqueId, linkIndex, visualShapeIndex, key);
return PyInt_FromLong(userDataId); return PyInt_FromLong(userDataId);
} }
@@ -797,16 +797,14 @@ static PyObject* pybullet_getUserData(PyObject* self, PyObject* args, PyObject*
{ {
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
int physicsClientId = 0; int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
int userDataId = -1; int userDataId = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataId", "physicsClientId", NULL}; static char* kwlist[] = {"userDataId", "physicsClientId", NULL};
struct b3UserDataValue value; struct b3UserDataValue value;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataId, &physicsClientId)) if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &userDataId, &physicsClientId))
{ {
return NULL; return NULL;
} }
@@ -818,7 +816,7 @@ static PyObject* pybullet_getUserData(PyObject* self, PyObject* args, PyObject*
} }
if (!b3GetUserData(sm, bodyUniqueId, linkIndex, userDataId, &value)) { if (!b3GetUserData(sm, userDataId, &value)) {
Py_INCREF(Py_None); Py_INCREF(Py_None);
return Py_None; return Py_None;
@@ -837,15 +835,14 @@ static PyObject* pybullet_getNumUserData(PyObject* self, PyObject* args, PyObjec
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
int physicsClientId = 0; int physicsClientId = 0;
int bodyUniqueId = -1; int bodyUniqueId = -1;
int linkIndex = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL}; static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL};
int numUserData; int numUserData;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId)) if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId))
{ {
return NULL; return NULL;
} }
@@ -856,7 +853,7 @@ static PyObject* pybullet_getNumUserData(PyObject* self, PyObject* args, PyObjec
return NULL; return NULL;
} }
numUserData = b3GetNumUserData(sm, bodyUniqueId, linkIndex); numUserData = b3GetNumUserData(sm, bodyUniqueId);
return PyInt_FromLong(numUserData); return PyInt_FromLong(numUserData);
} }
@@ -865,16 +862,17 @@ static PyObject* pybullet_getUserDataInfo(PyObject* self, PyObject* args, PyObje
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
int physicsClientId = 0; int physicsClientId = 0;
int bodyUniqueId = -1; int bodyUniqueId = -1;
int linkIndex = -1;
int userDataIndex = -1; int userDataIndex = -1;
int linkIndex = -1;
int visualShapeIndex = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataIndex", "physicsClientId", NULL}; static char* kwlist[] = {"bodyUniqueId", "userDataIndex", "physicsClientId", NULL};
const char* key = 0; const char* key = 0;
int userDataId = -1; int userDataId = -1;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataIndex, &physicsClientId)) if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &userDataIndex, &physicsClientId))
{ {
return NULL; return NULL;
} }
@@ -885,16 +883,19 @@ static PyObject* pybullet_getUserDataInfo(PyObject* self, PyObject* args, PyObje
return NULL; return NULL;
} }
b3GetUserDataInfo(sm, bodyUniqueId, linkIndex, userDataIndex, &key, &userDataId); b3GetUserDataInfo(sm, bodyUniqueId, userDataIndex, &key, &userDataId, &linkIndex, &visualShapeIndex);
if (key == 0 || userDataId == -1) { if (key == 0 || userDataId == -1) {
PyErr_SetString(SpamError, "Could not get user data info."); PyErr_SetString(SpamError, "Could not get user data info.");
return NULL; return NULL;
} }
{ {
PyObject *userDataInfoTuple = PyTuple_New(2); PyObject *userDataInfoTuple = PyTuple_New(5);
PyTuple_SetItem(userDataInfoTuple, 0, PyInt_FromLong(userDataId)); PyTuple_SetItem(userDataInfoTuple, 0, PyInt_FromLong(userDataId));
PyTuple_SetItem(userDataInfoTuple, 1, PyString_FromString(key)); PyTuple_SetItem(userDataInfoTuple, 1, PyString_FromString(key));
PyTuple_SetItem(userDataInfoTuple, 2, PyInt_FromLong(bodyUniqueId));
PyTuple_SetItem(userDataInfoTuple, 3, PyInt_FromLong(linkIndex));
PyTuple_SetItem(userDataInfoTuple, 4, PyInt_FromLong(visualShapeIndex));
return userDataInfoTuple; return userDataInfoTuple;
} }
} }
@@ -9131,28 +9132,28 @@ static PyMethodDef SpamMethods[] = {
"Update user data, in case other clients made changes."}, "Update user data, in case other clients made changes."},
{"addUserData", (PyCFunction)pybullet_addUserData, METH_VARARGS | METH_KEYWORDS, {"addUserData", (PyCFunction)pybullet_addUserData, METH_VARARGS | METH_KEYWORDS,
"addUserData(bodyUniqueId, linkIndex, key, value, physicsClientId=0)\n" "addUserData(bodyUniqueId, key, value, linkIndex=-1, visualShapeIndex=-1, physicsClientId=0)\n"
"Adds or updates a user data entry to a link. Returns user data identifier."}, "Adds or updates a user data entry. Returns user data identifier."},
{"getUserData", (PyCFunction)pybullet_getUserData, METH_VARARGS | METH_KEYWORDS, {"getUserData", (PyCFunction)pybullet_getUserData, METH_VARARGS | METH_KEYWORDS,
"getUserData(bodyUniqueId, linkIndex, userDataId, physicsClientId=0)\n" "getUserData(userDataId, physicsClientId=0)\n"
"Returns the user data value."}, "Returns the user data value."},
{"removeUserData", (PyCFunction)pybullet_removeUserData, METH_VARARGS | METH_KEYWORDS, {"removeUserData", (PyCFunction)pybullet_removeUserData, METH_VARARGS | METH_KEYWORDS,
"removeUserData(bodyUniqueId, linkIndex, userDataId, physicsClientId=0)\n" "removeUserData(userDataId, physicsClientId=0)\n"
"Removes a user data entry."}, "Removes a user data entry."},
{"getUserDataId", (PyCFunction)pybullet_getUserDataId, METH_VARARGS | METH_KEYWORDS, {"getUserDataId", (PyCFunction)pybullet_getUserDataId, METH_VARARGS | METH_KEYWORDS,
"getUserDataId(bodyUniqueId, linkIndex, key, physicsClientId=0)\n" "getUserDataId(bodyUniqueId, key, linkIndex=-1, visualShapeIndex=-1, physicsClientId=0)\n"
"Retrieves the userDataId on a link given the key."}, "Retrieves the userDataId given the key and optionally link and visual shape index."},
{"getNumUserData", (PyCFunction)pybullet_getNumUserData, METH_VARARGS | METH_KEYWORDS, {"getNumUserData", (PyCFunction)pybullet_getNumUserData, METH_VARARGS | METH_KEYWORDS,
"getNumUserData(bodyUniqueId, linkIndex, physicsClientId=0)\n" "getNumUserData(bodyUniqueId physicsClientId=0)\n"
"Retrieves the number of user data entries in a link."}, "Retrieves the number of user data entries in a body."},
{"getUserDataInfo", (PyCFunction)pybullet_getUserDataInfo, METH_VARARGS | METH_KEYWORDS, {"getUserDataInfo", (PyCFunction)pybullet_getUserDataInfo, METH_VARARGS | METH_KEYWORDS,
"getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, physicsClientId=0)\n" "getUserDataInfo(bodyUniqueId, userDataIndex, physicsClientId=0)\n"
"Retrieves the key and the identifier of a user data as (id, key)."}, "Retrieves the key and the identifier of a user data as (userDataId, key, bodyUniqueId, linkIndex, visualShapeIndex)."},
{"removeBody", (PyCFunction)pybullet_removeBody, METH_VARARGS | METH_KEYWORDS, {"removeBody", (PyCFunction)pybullet_removeBody, METH_VARARGS | METH_KEYWORDS,
"Remove a body by its body unique id."}, "Remove a body by its body unique id."},

235
examples/pybullet/unittests/userDataTest.py
View File

@@ -26,169 +26,170 @@ class TestUserDataMethods(unittest.TestCase):
def testAddUserData(self): def testAddUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH) plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
# Retrieve user data and make sure it's correct. # Retrieve user data and make sure it's correct.
self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1)) self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
self.assertEqual(b"MyValue2", self.client.getUserData(plane_id, 0, uid2)) self.assertEqual(b"MyValue2", self.client.getUserData(uid2))
self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3)) self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4)) self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
# Disconnect/reconnect and make sure that the user data is synced back. # Disconnect/reconnect and make sure that the user data is synced back.
del self.client del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1)) self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
self.assertEqual(b"MyValue2", self.client.getUserData(plane_id, 0, uid2)) self.assertEqual(b"MyValue2", self.client.getUserData(uid2))
self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3)) self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4)) self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
self.client.resetSimulation() self.client.resetSimulation()
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1)) self.assertEqual(None, self.client.getUserData(uid1))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3)) self.assertEqual(None, self.client.getUserData(uid3))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4)) self.assertEqual(None, self.client.getUserData(uid4))
def testGetNumUserData(self): def testGetNumUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH) plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
self.assertEqual(4, self.client.getNumUserData(plane_id, 0)) self.assertEqual(4, self.client.getNumUserData(plane_id))
del self.client del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(4, self.client.getNumUserData(plane_id, 0)) self.assertEqual(4, self.client.getNumUserData(plane_id))
def testReplaceUserData(self): def testReplaceUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH) plane_id = self.client.loadURDF(PLANE_PATH)
uid = self.client.addUserData(plane_id, 0, "MyKey", "MyValue") uid = self.client.addUserData(plane_id, "MyKey", "MyValue")
self.assertEqual(b"MyValue", self.client.getUserData(plane_id, 0, uid)) self.assertEqual(b"MyValue", self.client.getUserData(uid))
new_uid = self.client.addUserData(plane_id, 0, "MyKey", "MyNewValue") new_uid = self.client.addUserData(plane_id, "MyKey", "MyNewValue")
self.assertEqual(uid, new_uid) self.assertEqual(uid, new_uid)
self.assertEqual(b"MyNewValue", self.client.getUserData(plane_id, 0, uid)) self.assertEqual(b"MyNewValue", self.client.getUserData(uid))
del self.client del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(b"MyNewValue", self.client.getUserData(plane_id, 0, uid)) self.assertEqual(b"MyNewValue", self.client.getUserData(uid))
def testGetUserDataId(self): def testGetUserDataId(self):
plane_id = self.client.loadURDF(PLANE_PATH) plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
self.assertEqual(uid1, self.client.getUserDataId(plane_id, 0, "MyKey1")) self.assertEqual(uid1, self.client.getUserDataId(plane_id, "MyKey1"))
self.assertEqual(uid2, self.client.getUserDataId(plane_id, 0, "MyKey2")) self.assertEqual(uid2, self.client.getUserDataId(plane_id, "MyKey2"))
self.assertEqual(uid3, self.client.getUserDataId(plane_id, 0, "MyKey3")) self.assertEqual(uid3, self.client.getUserDataId(plane_id, "MyKey3"))
self.assertEqual(uid4, self.client.getUserDataId(plane_id, 0, "MyKey4")) self.assertEqual(uid4, self.client.getUserDataId(plane_id, "MyKey4"))
del self.client del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(uid1, self.client.getUserDataId(plane_id, 0, "MyKey1")) self.assertEqual(uid1, self.client.getUserDataId(plane_id, "MyKey1"))
self.assertEqual(uid2, self.client.getUserDataId(plane_id, 0, "MyKey2")) self.assertEqual(uid2, self.client.getUserDataId(plane_id, "MyKey2"))
self.assertEqual(uid3, self.client.getUserDataId(plane_id, 0, "MyKey3")) self.assertEqual(uid3, self.client.getUserDataId(plane_id, "MyKey3"))
self.assertEqual(uid4, self.client.getUserDataId(plane_id, 0, "MyKey4")) self.assertEqual(uid4, self.client.getUserDataId(plane_id, "MyKey4"))
def testRemoveUserData(self): def testRemoveUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH) plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
self.client.removeUserData(plane_id, 0, uid2) self.client.removeUserData(uid2)
self.assertEqual(3, self.client.getNumUserData(plane_id, 0)) self.assertEqual(3, self.client.getNumUserData(plane_id))
self.assertEqual(-1, self.client.getUserDataId(plane_id, 0, "MyKey2")) self.assertEqual(-1, self.client.getUserDataId(plane_id, "MyKey2"))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1)) self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3)) self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4)) self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
del self.client del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(3, self.client.getNumUserData(plane_id, 0)) self.assertEqual(3, self.client.getNumUserData(plane_id))
self.assertEqual(-1, self.client.getUserDataId(plane_id, 0, "MyKey2")) self.assertEqual(-1, self.client.getUserDataId(plane_id, "MyKey2"))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1)) self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3)) self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4)) self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
def testIterateAllUserData(self): def testIterateAllUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH) plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
entries = set() entries = set()
for i in range(self.client.getNumUserData(plane_id, 0)): for i in range(self.client.getNumUserData(plane_id)):
userDataId, key = self.client.getUserDataInfo(plane_id, 0, i) userDataId, key, bodyId, linkIndex, visualShapeIndex = self.client.getUserDataInfo(plane_id, i)
value = self.client.getUserData(plane_id, 0, userDataId); value = self.client.getUserData(userDataId);
entries.add((userDataId, key, value)) entries.add((userDataId, key, value, bodyId, linkIndex, visualShapeIndex))
self.assertTrue((uid1, b"MyKey1", b"MyValue1") in entries) self.assertTrue((uid1, b"MyKey1", b"MyValue1", plane_id, -1, -1) in entries)
self.assertTrue((uid2, b"MyKey2", b"MyValue2") in entries) self.assertTrue((uid2, b"MyKey2", b"MyValue2", plane_id, -1, -1) in entries)
self.assertTrue((uid3, b"MyKey3", b"MyValue3") in entries) self.assertTrue((uid3, b"MyKey3", b"MyValue3", plane_id, -1, -1) in entries)
self.assertTrue((uid4, b"MyKey4", b"MyValue4") in entries) self.assertTrue((uid4, b"MyKey4", b"MyValue4", plane_id, -1, -1) in entries)
self.assertEqual(4, len(entries)) self.assertEqual(4, len(entries))
def testRemoveBody(self): def testRemoveBody(self):
plane_id = self.client.loadURDF(PLANE_PATH) plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
self.client.removeBody(plane_id) self.client.removeBody(plane_id)
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1)) self.assertEqual(None, self.client.getUserData(uid1))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3)) self.assertEqual(None, self.client.getUserData(uid3))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4)) self.assertEqual(None, self.client.getUserData(uid4))
del self.client del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1)) self.assertEqual(None, self.client.getUserData(uid1))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3)) self.assertEqual(None, self.client.getUserData(uid3))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4)) self.assertEqual(None, self.client.getUserData(uid4))
def testMultipleBodies(self): def testMultipleBodies(self):
plane1 = self.client.loadURDF(PLANE_PATH) plane1 = self.client.loadURDF(PLANE_PATH)
plane2 = self.client.loadURDF(PLANE_PATH) plane2 = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane1, 0, "MyKey1", "This is plane 1 - 1") uid1 = self.client.addUserData(plane1, "MyKey1", "This is plane 1 - 1")
uid2 = self.client.addUserData(plane1, 0, "MyKey2", "This is plane 1 - 2") uid2 = self.client.addUserData(plane1, "MyKey2", "This is plane 1 - 2")
uid3 = self.client.addUserData(plane2, 0, "MyKey1", "This is plane 2 - 1") uid3 = self.client.addUserData(plane2, "MyKey1", "This is plane 2 - 1")
uid4 = self.client.addUserData(plane2, 0, "MyKey2", "This is plane 2 - 2") uid4 = self.client.addUserData(plane2, "MyKey2", "This is plane 2 - 2")
uid5 = self.client.addUserData(plane2, 0, "MyKey3", "This is plane 2 - 3") uid5 = self.client.addUserData(plane2, "MyKey3", "This is plane 2 - 3")
self.assertEqual(b"This is plane 1 - 1", self.client.getUserData(plane1, 0, self.client.getUserDataId(plane1, 0, "MyKey1"))) self.assertEqual(b"This is plane 1 - 1", self.client.getUserData(self.client.getUserDataId(plane1, "MyKey1")))
self.assertEqual(b"This is plane 1 - 2", self.client.getUserData(plane1, 0, self.client.getUserDataId(plane1, 0, "MyKey2"))) self.assertEqual(b"This is plane 1 - 2", self.client.getUserData(self.client.getUserDataId(plane1, "MyKey2")))
self.assertEqual(b"This is plane 2 - 1", self.client.getUserData(plane2, 0, self.client.getUserDataId(plane2, 0, "MyKey1"))) self.assertEqual(b"This is plane 2 - 1", self.client.getUserData(self.client.getUserDataId(plane2, "MyKey1")))
self.assertEqual(b"This is plane 2 - 2", self.client.getUserData(plane2, 0, self.client.getUserDataId(plane2, 0, "MyKey2"))) self.assertEqual(b"This is plane 2 - 2", self.client.getUserData(self.client.getUserDataId(plane2, "MyKey2")))
self.assertEqual(b"This is plane 2 - 3", self.client.getUserData(plane2, 0, self.client.getUserDataId(plane2, 0, "MyKey3"))) self.assertEqual(b"This is plane 2 - 3", self.client.getUserData(self.client.getUserDataId(plane2, "MyKey3")))
def testMultipleLinks(self): def testMultipleLinks(self):
@@ -198,14 +199,15 @@ class TestUserDataMethods(unittest.TestCase):
self.assertTrue(num_links > 1) self.assertTrue(num_links > 1)
for link_index in range(num_links): for link_index in range(num_links):
uid1 = self.client.addUserData(body_id, link_index, "MyKey1", "Value1 for link %s" % link_index) uid1 = self.client.addUserData(body_id, "MyKey1", "Value1 for link %s" % link_index, link_index)
uid2 = self.client.addUserData(body_id, link_index, "MyKey2", "Value2 for link %s" % link_index) uid2 = self.client.addUserData(body_id, "MyKey2", "Value2 for link %s" % link_index, link_index)
for link_index in range(num_links): for link_index in range(num_links):
uid1 = self.client.getUserDataId(body_id, link_index, "MyKey1") uid1 = self.client.getUserDataId(body_id, "MyKey1", link_index)
uid2 = self.client.getUserDataId(body_id, link_index, "MyKey2") uid2 = self.client.getUserDataId(body_id, "MyKey2", link_index)
self.assertEqual(("Value1 for link %s" % link_index).encode(), self.client.getUserData(body_id, link_index, uid1)) self.assertEqual(("Value1 for link %s" % link_index).encode(), self.client.getUserData(uid1))
self.assertEqual(("Value2 for link %s" % link_index).encode(), self.client.getUserData(body_id, link_index, uid2)) self.assertEqual(("Value2 for link %s" % link_index).encode(), self.client.getUserData(uid2))
def testMultipleClients(self): def testMultipleClients(self):
client1 = self.client client1 = self.client
@@ -215,25 +217,48 @@ class TestUserDataMethods(unittest.TestCase):
client2.syncBodyInfo() client2.syncBodyInfo()
# Add user data on client 1, check on client 1 # Add user data on client 1, check on client 1
uid = client1.addUserData(plane_id, 0, "MyKey", "MyValue") uid = client1.addUserData(plane_id, "MyKey", "MyValue")
self.assertEqual(None, client2.getUserData(plane_id, 0, uid)) self.assertEqual(None, client2.getUserData(uid))
client2.syncUserData() client2.syncUserData()
self.assertEqual(b"MyValue", client2.getUserData(plane_id, 0, uid)) self.assertEqual(b"MyValue", client2.getUserData(uid))
# Overwrite the value on client 2, check on client 1 # Overwrite the value on client 2, check on client 1
client2.addUserData(plane_id, 0, "MyKey", "MyNewValue") client2.addUserData(plane_id, "MyKey", "MyNewValue")
self.assertEqual(b"MyValue", client1.getUserData(plane_id, 0, uid)) self.assertEqual(b"MyValue", client1.getUserData(uid))
client1.syncUserData() client1.syncUserData()
self.assertEqual(b"MyNewValue", client1.getUserData(plane_id, 0, uid)) self.assertEqual(b"MyNewValue", client1.getUserData(uid))
# Remove user data on client 1, check on client 2 # Remove user data on client 1, check on client 2
client1.removeUserData(plane_id, 0, uid) client1.removeUserData(uid)
self.assertEqual(b"MyNewValue", client2.getUserData(plane_id, 0, uid)) self.assertEqual(b"MyNewValue", client2.getUserData(uid))
client2.syncUserData() client2.syncUserData()
self.assertEqual(None, client2.getUserData(plane_id, 0, uid)) self.assertEqual(None, client2.getUserData(uid))
del client2 del client2
def testUserDataOnVisualShapes(self):
body_id = self.client.loadURDF(ROBOT_PATH)
num_links = self.client.getNumJoints(body_id)
visual_shapes = self.client.getVisualShapeData(body_id)
self.assertTrue(num_links > 0)
self.assertTrue(len(visual_shapes) > 0)
user_data_entries = set()
for link_index in range(-1, num_links):
num_shapes = sum([1 for shape in visual_shapes if shape[1] == link_index])
for shape_index in range(num_shapes):
key = "MyKey"
value = "MyValue %s, %s" % (link_index, shape_index)
uid = self.client.addUserData(body_id, key, value, link_index, shape_index)
user_data_entries.add((uid, key, value.encode(), body_id, link_index, shape_index))
self.assertEqual(len(visual_shapes), self.client.getNumUserData(body_id))
for uid, key, value, body_id, link_index, shape_index in user_data_entries:
self.assertEqual(value, self.client.getUserData(uid))
self.assertEqual(uid, self.client.getUserDataId(body_id, key, link_index, shape_index))
if __name__ == "__main__": if __name__ == "__main__":
unittest.main() unittest.main()