From 9c7aa3a863447c3dd323515266576ccb05ae963f Mon Sep 17 00:00:00 2001 From: Tigran Gasparian Date: Tue, 3 Jul 2018 17:45:19 +0200 Subject: [PATCH] Changes UserData to use global identifiers and makes linkIndex optional. 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) --- examples/SharedMemory/PhysicsClient.h | 8 +- examples/SharedMemory/PhysicsClientC_API.cpp | 25 +- examples/SharedMemory/PhysicsClientC_API.h | 12 +- .../PhysicsClientSharedMemory.cpp | 155 +++++------- .../SharedMemory/PhysicsClientSharedMemory.h | 8 +- examples/SharedMemory/PhysicsDirect.cpp | 141 ++++------- examples/SharedMemory/PhysicsDirect.h | 8 +- examples/SharedMemory/PhysicsLoopBack.cpp | 18 +- examples/SharedMemory/PhysicsLoopBack.h | 8 +- .../PhysicsServerCommandProcessor.cpp | 197 +++++---------- examples/SharedMemory/SharedMemoryCommands.h | 18 +- examples/SharedMemory/SharedMemoryPublic.h | 8 +- examples/SharedMemory/SharedMemoryUserData.h | 53 +++- examples/pybullet/examples/userData.py | 66 ++--- examples/pybullet/pybullet.c | 73 +++--- examples/pybullet/unittests/userDataTest.py | 235 ++++++++++-------- 16 files changed, 478 insertions(+), 555 deletions(-) diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 6dd537fd6..2b9357155 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -75,10 +75,10 @@ public: virtual void setTimeOut(double timeOutInSeconds) = 0; virtual double getTimeOut() const = 0; - virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const = 0; - virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const = 0; - virtual int getNumUserData(int bodyUniqueId, int linkIndex) const = 0; - virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const = 0; + virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const = 0; + virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const = 0; + virtual int getNumUserData(int bodyUniqueId) 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 popProfileTiming()=0; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e5abaa26a..d88e15d42 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2848,7 +2848,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsCl 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; b3Assert(strlen(key) < MAX_USER_DATA_KEY_LENGTH); b3Assert(cl); @@ -2860,6 +2860,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsCli command->m_type = CMD_ADD_USER_DATA; command->m_addUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId; command->m_addUserDataRequestArgs.m_linkIndex = linkIndex; + command->m_addUserDataRequestArgs.m_visualShapeIndex = visualShapeIndex; command->m_addUserDataRequestArgs.m_valueType = valueType; command->m_addUserDataRequestArgs.m_valueLength = valueLength; strcpy(command->m_addUserDataRequestArgs.m_key, key); @@ -2868,7 +2869,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsCli 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; b3Assert(cl); b3Assert(cl->canSubmitCommand()); @@ -2876,29 +2877,27 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3Physics b3Assert(command); 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; 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; if (cl) { - return cl->getCachedUserData(bodyUniqueId, linkIndex, userDataId, *valueOut); + return cl->getCachedUserData(userDataId, *valueOut); } 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; if (cl) { - return cl->getCachedUserDataId(bodyUniqueId, linkIndex, key); + return cl->getCachedUserDataId(bodyUniqueId, linkIndex, visualShapeIndex, key); } return -1; } @@ -2909,27 +2908,27 @@ B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHan if (status) { 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; } -B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex) +B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId) { PhysicsClient* cl = (PhysicsClient*)physClient; if (cl) { - return cl->getNumUserData(bodyUniqueId, linkIndex); + return cl->getNumUserData(bodyUniqueId); } 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; if (cl) { - cl->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut); + cl->getUserDataInfo(bodyUniqueId, userDataIndex, keyOut, userDataIdOut, linkIndexOut, visualShapeIndexOut); } } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index e57aaa510..2870f7f1e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -115,14 +115,14 @@ B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqu ///user data handling 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 b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId); +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 userDataId); -B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue *valueOut); -B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char *key); +B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int userDataId, struct b3UserDataValue *valueOut); +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 b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); -B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut); +B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId); +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); ///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 1f3d24c35..f787fec05 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -15,26 +15,12 @@ #include "LinearMath/btQuickprof.h" -struct UserDataCache -{ - btHashMap m_userDataMap; - btHashMap m_keyToUserDataIdMap; - - UserDataCache() - { - } - ~UserDataCache() - { - } -}; - struct BodyJointInfoCache { std::string m_baseName; b3AlignedObjectArray m_jointInfo; std::string m_bodyName; - // Joint index -> user data. - btHashMap m_jointToUserDataMap; + btAlignedObjectArray m_userDataIds; ~BodyJointInfoCache() { @@ -78,7 +64,10 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_bodyIdsRequestInfo; btAlignedObjectArray m_constraintIdsRequestInfo; - btAlignedObjectArray m_userDataIdsRequestInfo; + btAlignedObjectArray m_userDataIdsRequestInfo; + btHashMap m_userDataMap; + btHashMap m_userDataHandleLookup; + SharedMemoryStatus m_tempBackupServerStatus; @@ -243,6 +232,12 @@ void PhysicsClientSharedMemory::removeCachedBody(int bodyUniqueId) BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; 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); m_data->m_bodyJointMap.remove(bodyUniqueId); } @@ -264,6 +259,8 @@ void PhysicsClientSharedMemory::resetData() } m_data->m_bodyJointMap.clear(); m_data->m_userConstraintInfoMap.clear(); + m_data->m_userDataHandleLookup.clear(); + m_data->m_userDataMap.clear(); } void PhysicsClientSharedMemory::setSharedMemoryKey(int key) @@ -1409,25 +1406,26 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); 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; if (numIdentifiers > 0) { m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus; - const b3UserDataGlobalIdentifier *identifiers = (b3UserDataGlobalIdentifier *)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; - for (int i=0; im_testBlock1->m_bulletStreamDataServerToClientRefactor; + m_data->m_userDataIdsRequestInfo.reserve(numIdentifiers - 1); + // Store the identifiers that still need to be requested. + for (int i=0; im_userDataIdsRequestInfo.push_back(identifiers[i]); } - // Request individual user data entries. - const b3UserDataGlobalIdentifier userDataGlobalId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1]; - m_data->m_userDataIdsRequestInfo.pop_back(); - + // Request individual user data entries, start with last identifier. SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; command.m_type = CMD_REQUEST_USER_DATA; - command.m_userDataRequestArgs = userDataGlobalId; + command.m_userDataRequestArgs.m_userDataId = identifiers[numIdentifiers - 1]; submitClientCommand(command); 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) { B3_PROFILE("CMD_ADD_USER_DATA_COMPLETED"); - const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId; - BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; + const UserDataResponseArgs response = serverCmd.m_userDataResponseArgs; + BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[response.m_bodyUniqueId]; 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; - - SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; - if (userDataPtr) { + SharedMemoryUserData* userData = m_data->m_userDataMap[response.m_userDataId]; + if (userData) { // 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 { // Add a new user data entry. - const char *key = serverCmd.m_userDataResponseArgs.m_key; - (userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(key)); - (userDataCachePtr)->m_keyToUserDataIdMap.insert(key, userDataGlobalId.m_userDataId); - userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; - userDataPtr->replaceValue(dataStream, serverCmd.m_userDataResponseArgs.m_valueLength, serverCmd.m_userDataResponseArgs.m_valueType); + const char *key = response.m_key; + m_data->m_userDataMap.insert(response.m_userDataId, SharedMemoryUserData(key, response.m_bodyUniqueId, response.m_linkIndex, response.m_visualShapeIndex)); + userData = m_data->m_userDataMap[response.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); } } } @@ -1468,12 +1458,12 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { if (m_data->m_userDataIdsRequestInfo.size() > 0) { // 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(); SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; command.m_type = CMD_REQUEST_USER_DATA; - command.m_userDataRequestArgs = userDataGlobalId; + command.m_userDataRequestArgs.m_userDataId = userDataId; submitClientCommand(command); return 0; } @@ -1482,19 +1472,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { if (serverCmd.m_type == CMD_REMOVE_USER_DATA_COMPLETED) { B3_PROFILE("CMD_REMOVE_USER_DATA_COMPLETED"); - const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs; - BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; - if (bodyJointsPtr && *bodyJointsPtr) { - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; - 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); - } + const int userDataId = serverCmd.m_removeUserDataResponseArgs.m_userDataId; + SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId]; + if (userData) { + BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userData->m_bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) { + (*bodyJointsPtr)->m_userDataIds.remove(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; } -bool PhysicsClientSharedMemory::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const { - BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; - if (!bodyJointsPtr || !(*bodyJointsPtr)) { - return false; - } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr) { - return false; - } - SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId]; +bool PhysicsClientSharedMemory::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const { + SharedMemoryUserData *userDataPtr = m_data->m_userDataMap[userDataId]; if (!userDataPtr) { return false; @@ -1872,54 +1850,37 @@ bool PhysicsClientSharedMemory::getCachedUserData(int bodyUniqueId, int linkInde return true; } -int PhysicsClientSharedMemory::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { - BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; - if (!bodyJointsPtr || !(*bodyJointsPtr)) { - return -1; - } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr) - { - return -1; - } - int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key]; +int PhysicsClientSharedMemory::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const { + int* userDataId = m_data->m_userDataHandleLookup.find(SharedMemoryUserDataHashKey(key, bodyUniqueId, linkIndex, visualShapeIndex)); if (!userDataId) { return -1; } return *userDataId; } -int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId, int linkIndex) const { +int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId) const { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (!bodyJointsPtr || !(*bodyJointsPtr)) { return 0; } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr) - { - return 0; - } - return (userDataCachePtr)->m_userDataMap.size(); + return (*bodyJointsPtr)->m_userDataIds.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]; - 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; - *userDataIdOut = -1; - return; - } - *userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1(); - SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex); - *keyOut = (userDataPtr)->m_key.c_str(); + int userDataId = (*bodyJointsPtr)->m_userDataIds[userDataIndex]; + SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId]; + + *userDataIdOut = userDataId; + *keyOut = userData->m_key.c_str(); + *linkIndexOut = userData->m_linkIndex; + *visualShapeIndexOut = userData->m_visualShapeIndex; } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index 10c1c63fb..37d30a55e 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -85,10 +85,10 @@ public: virtual void setTimeOut(double timeOutInSeconds); virtual double getTimeOut() const; - virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const; - virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const; - virtual int getNumUserData(int bodyUniqueId, int linkIndex) const; - virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const; + virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const; + virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const; + virtual int getNumUserData(int bodyUniqueId) 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 popProfileTiming(); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 45f85d75c..f1fb24548 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -16,24 +16,13 @@ #include "SharedMemoryUserData.h" #include "LinearMath/btQuickprof.h" -struct UserDataCache { - btHashMap m_userDataMap; - btHashMap m_keyToUserDataIdMap; - - ~UserDataCache() - { - - } -}; struct BodyJointInfoCache2 { std::string m_baseName; btAlignedObjectArray m_jointInfo; std::string m_bodyName; - - // Joint index -> user data. - btHashMap m_jointToUserDataMap; + btAlignedObjectArray m_userDataIds; ~BodyJointInfoCache2() { } @@ -85,6 +74,9 @@ struct PhysicsDirectInternalData btAlignedObjectArray m_raycastHits; + btHashMap m_userDataMap; + btHashMap m_userDataHandleLookup; + PhysicsCommandProcessorInterface* m_commandProcessor; bool m_ownsCommandProcessor; double m_timeOutInSeconds; @@ -685,34 +677,23 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta } void PhysicsDirect::processAddUserData(const struct SharedMemoryStatus& serverCmd) { - const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId; - BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; - 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 UserDataResponseArgs response = serverCmd.m_userDataResponseArgs; + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[response.m_bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) { const char *dataStream = m_data->m_bulletStreamDataServerToClient; - - b3UserDataValue userDataValue; - 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) { + SharedMemoryUserData* userData = m_data->m_userDataMap[response.m_userDataId]; + if (userData) { // 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 { // Add a new user data entry. - (userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(serverCmd.m_userDataResponseArgs.m_key)); - userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; - userDataPtr->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type); - (userDataCachePtr)->m_keyToUserDataIdMap.insert(serverCmd.m_userDataResponseArgs.m_key, userDataGlobalId.m_userDataId); + const char *key = response.m_key; + m_data->m_userDataMap.insert(response.m_userDataId, SharedMemoryUserData(key, response.m_bodyUniqueId, response.m_linkIndex, response.m_visualShapeIndex)); + userData = m_data->m_userDataMap[response.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); 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; - b3UserDataGlobalIdentifier *identifiers = new b3UserDataGlobalIdentifier[numIdentifiers]; - memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(b3UserDataGlobalIdentifier)); + int *identifiers = new int[numIdentifiers]; + memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(int)); for (int i=0; im_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); @@ -1184,18 +1167,15 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd } case CMD_REMOVE_USER_DATA_COMPLETED: { - const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs; - BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; - if (bodyJointsPtr && *bodyJointsPtr) { - UserDataCache *userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; - 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); - } + const int userDataId = serverCmd.m_removeUserDataResponseArgs.m_userDataId; + SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId]; + if (userData) { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userData->m_bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) { + (*bodyJointsPtr)->m_userDataIds.remove(userDataId); } + m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData)); + m_data->m_userDataMap.remove(userDataId); } break; } @@ -1253,6 +1233,12 @@ void PhysicsDirect::removeCachedBody(int bodyUniqueId) BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; 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); m_data->m_bodyJointMap.remove(bodyUniqueId); } @@ -1499,72 +1485,49 @@ double PhysicsDirect::getTimeOut() const return m_data->m_timeOutInSeconds; } -bool PhysicsDirect::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const { - BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; - if (!bodyJointsPtr || !(*bodyJointsPtr)) { - return false; - } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr) - { - return false; - } - SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId]; +bool PhysicsDirect::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const { + SharedMemoryUserData *userDataPtr = m_data->m_userDataMap[userDataId]; if (!userDataPtr) { return false; } - valueOut.m_type = userDataPtr->m_type; + valueOut.m_type = (userDataPtr)->m_type; valueOut.m_length = userDataPtr->m_bytes.size(); valueOut.m_data1 = userDataPtr->m_bytes.size()? &userDataPtr->m_bytes[0] : 0; return true; } -int PhysicsDirect::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { - BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; - if (!bodyJointsPtr || !(*bodyJointsPtr)) { - return -1; - } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr) { - return -1; - } - int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key]; +int PhysicsDirect::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const { + int* userDataId = m_data->m_userDataHandleLookup.find(SharedMemoryUserDataHashKey(key, bodyUniqueId, linkIndex, visualShapeIndex)); if (!userDataId) { return -1; } return *userDataId; } -int PhysicsDirect::getNumUserData(int bodyUniqueId, int linkIndex) const { +int PhysicsDirect::getNumUserData(int bodyUniqueId) const { BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (!bodyJointsPtr || !(*bodyJointsPtr)) { return 0; } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr) { - return 0; - } - return (userDataCachePtr)->m_userDataMap.size(); + return (*bodyJointsPtr)->m_userDataIds.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]; - if (!bodyJointsPtr || !(*bodyJointsPtr)) { - *keyOut = 0; - *userDataIdOut = -1; - return; - } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size()) + if (!bodyJointsPtr || !(*bodyJointsPtr) || userDataIndex <= 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size()) { *keyOut = 0; *userDataIdOut = -1; return; } - *userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1(); - SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex); - *keyOut = (userDataPtr)->m_key.c_str(); + int userDataId = (*bodyJointsPtr)->m_userDataIds[userDataIndex]; + SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId]; + + *userDataIdOut = userDataId; + *keyOut = userData->m_key.c_str(); + *linkIndexOut = userData->m_linkIndex; + *visualShapeIndexOut = userData->m_visualShapeIndex; } diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 2b4a9d634..f0c75e750 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -115,10 +115,10 @@ public: virtual void setTimeOut(double timeOutInSeconds); virtual double getTimeOut() const; - virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const; - virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const; - virtual int getNumUserData(int bodyUniqueId, int linkIndex) const; - virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const; + virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const; + virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const; + virtual int getNumUserData(int bodyUniqueId) 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 popProfileTiming(); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 4a4b00d70..f615f0903 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -231,20 +231,20 @@ double PhysicsLoopBack::getTimeOut() const return m_data->m_physicsClient->getTimeOut(); } -bool PhysicsLoopBack::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const { - return m_data->m_physicsClient->getCachedUserData(bodyUniqueId, linkIndex, userDataId, valueOut); +bool PhysicsLoopBack::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const { + return m_data->m_physicsClient->getCachedUserData(userDataId, valueOut); } -int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { - return m_data->m_physicsClient->getCachedUserDataId(bodyUniqueId, linkIndex, key); +int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const { + return m_data->m_physicsClient->getCachedUserDataId(bodyUniqueId, linkIndex, visualShapeIndex, key); } -int PhysicsLoopBack::getNumUserData(int bodyUniqueId, int linkIndex) const { - return m_data->m_physicsClient->getNumUserData(bodyUniqueId, linkIndex); +int PhysicsLoopBack::getNumUserData(int bodyUniqueId) const { + return m_data->m_physicsClient->getNumUserData(bodyUniqueId); } -void PhysicsLoopBack::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const { - m_data->m_physicsClient->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut); +void PhysicsLoopBack::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const { + m_data->m_physicsClient->getUserDataInfo(bodyUniqueId, userDataIndex, keyOut, userDataIdOut, linkIndexOut, visualShapeIndexOut); } void PhysicsLoopBack::pushProfileTiming(const char* timingName) @@ -254,4 +254,4 @@ void PhysicsLoopBack::pushProfileTiming(const char* timingName) void PhysicsLoopBack::popProfileTiming() { m_data->m_physicsClient->popProfileTiming(); -} \ No newline at end of file +} diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index e18fa22f8..d5449452e 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -89,10 +89,10 @@ public: virtual void setTimeOut(double timeOutInSeconds); virtual double getTimeOut() const; - virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const; - virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const; - virtual int getNumUserData(int bodyUniqueId, int linkIndex) const; - virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const; + virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const; + virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const; + virtual int getNumUserData(int bodyUniqueId) 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 popProfileTiming(); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d3c515834..502d757f5 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -222,72 +222,6 @@ struct InternalCollisionShapeData #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 m_keyToHandleMap; - b3ResizablePool > 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 &userDataIds) const - { - m_userData.getUsedHandles(userDataIds); - } -}; - struct InternalBodyData { btMultiBody* m_multiBody; @@ -303,7 +237,7 @@ struct InternalBodyData btAlignedObjectArray m_rigidBodyJoints; btAlignedObjectArray m_rigidBodyJointNames; btAlignedObjectArray m_rigidBodyLinkNames; - btHashMap m_linkUserDataMap; + btAlignedObjectArray m_userDataHandles; #ifdef B3_ENABLE_TINY_AUDIO b3HashMap m_audioSources; @@ -328,10 +262,7 @@ struct InternalBodyData m_rigidBodyJoints.clear(); m_rigidBodyJointNames.clear(); m_rigidBodyLinkNames.clear(); - for(int i=0; i m_bodyHandles; b3ResizablePool m_userCollisionShapeHandles; b3ResizablePool m_userVisualShapeHandles; - - + b3ResizablePool> m_userDataHandles; + btHashMap m_userDataHandleLookup; b3PluginManager m_pluginManager; @@ -5042,34 +4973,11 @@ bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct Shar bool hasStatus = true; BT_PROFILE("CMD_SYNC_USER_DATA"); - b3UserDataGlobalIdentifier *userDataIdentifiers = (b3UserDataGlobalIdentifier *)bufferServerToClient; - int numIdentifiers = 0; - b3AlignedObjectArray bodyHandles; - m_data->m_bodyHandles.getUsedHandles(bodyHandles); - for (int i=0; im_bodyHandles.getHandle(bodyHandle); - if (!body) { - continue; - } - for (int j=0; jm_linkUserDataMap.size(); j++) { - const int linkIndex = body->m_linkUserDataMap.getKeyAtIndex(j).getUid1(); - InternalLinkUserData **userDataPtr = body->m_linkUserDataMap.getAtIndex(j); - if (!userDataPtr) { - continue; - } - b3AlignedObjectArray userDataIds; - (*userDataPtr)->getUserDataIds(userDataIds); - for (int k=0; k userDataHandles; + m_data->m_userDataHandles.getUsedHandles(userDataHandles); + memcpy(bufferServerToClient, &userDataHandles[0], sizeof(int) * userDataHandles.size()); - serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = numIdentifiers; + serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = userDataHandles.size(); serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED; return hasStatus; } @@ -5080,21 +4988,16 @@ bool PhysicsServerCommandProcessor::processRequestUserDataCommand(const struct S BT_PROFILE("CMD_REQUEST_USER_DATA"); serverStatusOut.m_type = CMD_REQUEST_USER_DATA_FAILED; - InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_userDataRequestArgs.m_bodyUniqueId); - 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); + SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(clientCmd.m_userDataRequestArgs.m_userDataId); if (!userData) { return hasStatus; } - btAssert(bufferSizeInBytes >= userData->m_bytes.size()); - serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId = clientCmd.m_userDataRequestArgs; + + btAssert(bufferSizeInBytes >= userData->m_bytes.size()); + 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_valueLength = userData->m_bytes.size(); serverStatusOut.m_type = CMD_REQUEST_USER_DATA_COMPLETED; @@ -5117,27 +5020,43 @@ bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct Share { return hasStatus; } - + InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId); if (!body) { return hasStatus; } - const int linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex; - InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex]; - if (!userDataPtr) { - InternalLinkUserData *userData = new InternalLinkUserData; - userDataPtr = &userData; - body->m_linkUserDataMap.insert(linkIndex, userData); + + SharedMemoryUserDataHashKey userDataIdentifier( + clientCmd.m_addUserDataRequestArgs.m_key, + clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId, + clientCmd.m_addUserDataRequestArgs.m_linkIndex, + 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, - bufferServerToClient,clientCmd.m_addUserDataRequestArgs.m_valueLength, + if (!userDataHandlePtr) { + 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); serverStatusOut.m_type = CMD_ADD_USER_DATA_COMPLETED; - serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_userDataId = userDataId; - serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId; - serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex; + serverStatusOut.m_userDataResponseArgs.m_userDataId = userDataHandle; + serverStatusOut.m_userDataResponseArgs.m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId; + 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_valueType = clientCmd.m_addUserDataRequestArgs.m_valueType; 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"); 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) { return hasStatus; } - const int linkIndex = clientCmd.m_removeUserDataRequestArgs.m_linkIndex; - InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex]; - if (!userDataPtr) { - return hasStatus; - } - const bool removed = (*userDataPtr)->remove(clientCmd.m_removeUserDataRequestArgs.m_userDataId); - if (!removed) { - return hasStatus; - } + body->m_userDataHandles.remove(clientCmd.m_removeUserDataRequestArgs.m_userDataId); + + m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData)); + m_data->m_userDataHandles.freeHandle(clientCmd.m_removeUserDataRequestArgs.m_userDataId); + serverStatusOut.m_removeUserDataResponseArgs = clientCmd.m_removeUserDataRequestArgs; serverStatusOut.m_type = CMD_REMOVE_USER_DATA_COMPLETED; return hasStatus; @@ -8278,6 +8198,12 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared bodyHandle->m_rigidBody=0; 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); } @@ -10495,6 +10421,9 @@ void PhysicsServerCommandProcessor::resetSimulation() m_data->m_userCollisionShapeHandles.exitHandles(); m_data->m_userCollisionShapeHandles.initHandles(); + m_data->m_userDataHandles.exitHandles(); + m_data->m_userDataHandles.initHandles(); + m_data->m_userDataHandleLookup.clear(); } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b94ed8666..e529f66b1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -993,13 +993,20 @@ struct b3StateSerializationArguments struct SyncUserDataArgs { // User data identifiers stored in m_bulletStreamDataServerToClientRefactor - // as as array of b3UserDataGlobalIdentifier objects + // as as array of integers. int m_numUserDataIdentifiers; }; +struct UserDataRequestArgs { + int m_userDataId; +}; + struct UserDataResponseArgs { - b3UserDataGlobalIdentifier m_userDataGlobalId; + int m_userDataId; + int m_bodyUniqueId; + int m_linkIndex; + int m_visualShapeIndex; int m_valueType; int m_valueLength; char m_key[MAX_USER_DATA_KEY_LENGTH]; @@ -1010,6 +1017,7 @@ struct AddUserDataRequestArgs { int m_bodyUniqueId; int m_linkIndex; + int m_visualShapeIndex; int m_valueType; int m_valueLength; char m_key[MAX_USER_DATA_KEY_LENGTH]; @@ -1073,9 +1081,9 @@ struct SharedMemoryCommand struct b3CustomCommand m_customCommandArgs; struct b3StateSerializationArguments m_loadStateArguments; struct RequestCollisionShapeDataArgs m_requestCollisionShapeDataArguments; - struct b3UserDataGlobalIdentifier m_userDataRequestArgs; + struct UserDataRequestArgs m_userDataRequestArgs; struct AddUserDataRequestArgs m_addUserDataRequestArgs; - struct b3UserDataGlobalIdentifier m_removeUserDataRequestArgs; + struct UserDataRequestArgs m_removeUserDataRequestArgs; }; }; @@ -1150,7 +1158,7 @@ struct SharedMemoryStatus struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs; struct SyncUserDataArgs m_syncUserDataArgs; struct UserDataResponseArgs m_userDataResponseArgs; - struct b3UserDataGlobalIdentifier m_removeUserDataResponseArgs; + struct UserDataRequestArgs m_removeUserDataResponseArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7e7338e36..86d55db3c 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -274,6 +274,7 @@ struct b3JointInfo enum UserDataValueType { + USER_DATA_VALUE_TYPE_NOT_USET = -1, // Data represents generic byte array. USER_DATA_VALUE_TYPE_BYTES = 0, // Data represents C-string @@ -287,13 +288,6 @@ struct b3UserDataValue char* m_data1; }; -struct b3UserDataGlobalIdentifier -{ - int m_bodyUniqueId; - int m_linkIndex; - int m_userDataId; -}; - struct b3UserConstraint { int m_parentBodyIndex; diff --git a/examples/SharedMemory/SharedMemoryUserData.h b/examples/SharedMemory/SharedMemoryUserData.h index a29f59eb0..991714557 100644 --- a/examples/SharedMemory/SharedMemoryUserData.h +++ b/examples/SharedMemory/SharedMemoryUserData.h @@ -3,6 +3,7 @@ #include #include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btHashMap.h" #include "SharedMemoryPublic.h" struct SharedMemoryUserData @@ -10,20 +11,22 @@ struct SharedMemoryUserData std::string m_key; int m_type; + int m_bodyUniqueId; + int m_linkIndex; + int m_visualShapeIndex; + btAlignedObjectArray m_bytes; 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) - :m_key(key) + SharedMemoryUserData(const char* key, int bodyUniqueId, int linkIndex, int visualShapeIndex) + :m_key(key), m_type(-1), m_bodyUniqueId(bodyUniqueId), m_linkIndex(linkIndex), m_visualShapeIndex(visualShapeIndex) { } - // Takes ownership of the data pointed to by newValue. void replaceValue(const char* bytes, int len, int 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 diff --git a/examples/pybullet/examples/userData.py b/examples/pybullet/examples/userData.py index 17080a4e5..164c470b5 100644 --- a/examples/pybullet/examples/userData.py +++ b/examples/pybullet/examples/userData.py @@ -21,16 +21,16 @@ plane_id = client.loadURDF(PLANE_PATH) print ("Plane ID: %s" % plane_id) print ("Adding user data to plane") -MyKey1 = client.addUserData(plane_id, 0, "MyKey1", "MyValue1") -MyKey2 = client.addUserData(plane_id, 0, "MyKey2", "MyValue2") -MyKey3 = client.addUserData(plane_id, 0, "MyKey3", "MyValue3") -MyKey4 = client.addUserData(plane_id, 0, "MyKey4", "MyValue4") +MyKey1 = client.addUserData(plane_id, "MyKey1", "MyValue1") +MyKey2 = client.addUserData(plane_id, "MyKey2", "MyValue2") +MyKey3 = client.addUserData(plane_id, "MyKey3", "MyValue3") +MyKey4 = client.addUserData(plane_id, "MyKey4", "MyValue4") print ("Retrieving cached user data") -print (client.getUserData(plane_id, 0, MyKey1)) -print (client.getUserData(plane_id, 0, MyKey2)) -print (client.getUserData(plane_id, 0, MyKey3)) -print (client.getUserData(plane_id, 0, MyKey4)) +print (client.getUserData(MyKey1)) +print (client.getUserData(MyKey2)) +print (client.getUserData(MyKey3)) +print (client.getUserData(MyKey4)) print ("Disconnecting") del client @@ -39,18 +39,18 @@ print ("Reconnecting") client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD) print ("Retrieving synced user data") -print (client.getUserData(plane_id, 0, MyKey1)) -print (client.getUserData(plane_id, 0, MyKey2)) -print (client.getUserData(plane_id, 0, MyKey3)) -print (client.getUserData(plane_id, 0, MyKey4)) +print (client.getUserData(MyKey1)) +print (client.getUserData(MyKey2)) +print (client.getUserData(MyKey3)) +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") -client.addUserData(plane_id, 0, "MyKey1", "MyNewValue") +client.addUserData(plane_id, "MyKey1", "MyNewValue") print ("Cached overridden data") -print (client.getUserData(plane_id, 0, MyKey1)) +print (client.getUserData(MyKey1)) print ("Disconnecting") @@ -61,50 +61,50 @@ client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD) print ("Synced overridden data") -print (client.getUserData(plane_id, 0, MyKey1)) +print (client.getUserData(MyKey1)) 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") -client.removeUserData(plane_id, 0, MyKey2) +client.removeUserData(MyKey2) print ("Retrieving cached removed data") -print (client.getUserData(plane_id, 0, MyKey2)) +print (client.getUserData(MyKey2)) print ("Syncing") client.syncUserData() 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") -for i in range(client.getNumUserData(plane_id, 0)): - userDataId, key = client.getUserDataInfo(plane_id, 0, i) - print ("Info: (%s, %s)" % (userDataId, key)) - print ("Value: %s" % client.getUserData(plane_id, 0, userDataId)) +for i in range(client.getNumUserData(plane_id)): + userDataId, key, bodyId, linkIndex, visualShapeIndex = client.getUserDataInfo(plane_id, i) + print ("Info: (%s, %s, %s, %s, %s)" % (userDataId, key, bodyId, linkIndex, visualShapeIndex)) + print ("Value: %s" % client.getUserData(userDataId)) print ("Removing body") client.removeBody(plane_id) print ("Retrieving user data") -print (client.getUserData(plane_id, 0, MyKey1)) -print (client.getUserData(plane_id, 0, MyKey3)) -print (client.getUserData(plane_id, 0, MyKey4)) +print (client.getUserData(MyKey1)) +print (client.getUserData(MyKey3)) +print (client.getUserData(MyKey4)) print ("Syncing") client.syncUserData() print ("Retrieving user data") -print (client.getUserData(plane_id, 0, MyKey1)) -print (client.getUserData(plane_id, 0, MyKey3)) -print (client.getUserData(plane_id, 0, MyKey4)) +print (client.getUserData(MyKey1)) +print (client.getUserData(MyKey3)) +print (client.getUserData(MyKey4)) plane_id2 = client.loadURDF(PLANE_PATH) print ("Plane1: %s, plane2: %s" % (plane_id, plane_id2)) print ("Retrieving user data") -print (client.getUserData(plane_id, 0, MyKey1)) -print (client.getUserData(plane_id, 0, MyKey3)) -print (client.getUserData(plane_id, 0, MyKey4)) +print (client.getUserData(MyKey1)) +print (client.getUserData(MyKey3)) +print (client.getUserData(MyKey4)) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b7c704305..a319ab937 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -689,18 +689,19 @@ static PyObject* pybullet_addUserData(PyObject* self, PyObject* args, PyObject* b3PhysicsClientHandle sm = 0; int physicsClientId = 0; int bodyUniqueId = -1; - int linkIndex = -2; + int linkIndex = -1; + int visualShapeIndex = -1; const char* key = ""; 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; b3SharedMemoryStatusHandle statusHandle; int statusType; int userDataId; 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; } @@ -712,7 +713,7 @@ static PyObject* pybullet_addUserData(PyObject* self, PyObject* args, PyObject* } 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); statusType = b3GetStatusType(statusHandle); @@ -730,16 +731,14 @@ static PyObject* pybullet_removeUserData(PyObject* self, PyObject* args, PyObjec { b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - int bodyUniqueId = -1; - int linkIndex = -1; int userDataId = -1; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataId", "physicsClientId", NULL}; + static char* kwlist[] = {"userDataId", "physicsClientId", NULL}; b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; 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; } @@ -750,7 +749,7 @@ static PyObject* pybullet_removeUserData(PyObject* self, PyObject* args, PyObjec return NULL; } - command = b3InitRemoveUserDataCommand(sm, bodyUniqueId, linkIndex, userDataId); + command = b3InitRemoveUserDataCommand(sm, userDataId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); @@ -770,15 +769,16 @@ static PyObject* pybullet_getUserDataId(PyObject* self, PyObject* args, PyObject int physicsClientId = 0; int bodyUniqueId = -1; int linkIndex = -1; + int visualShapeIndex = -1; const char* key = ""; 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; } @@ -789,7 +789,7 @@ static PyObject* pybullet_getUserDataId(PyObject* self, PyObject* args, PyObject return NULL; } - userDataId = b3GetUserDataId(sm, bodyUniqueId, linkIndex, key); + userDataId = b3GetUserDataId(sm, bodyUniqueId, linkIndex, visualShapeIndex, key); return PyInt_FromLong(userDataId); } @@ -797,16 +797,14 @@ static PyObject* pybullet_getUserData(PyObject* self, PyObject* args, PyObject* { b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - int bodyUniqueId = -1; - int linkIndex = -1; int userDataId = -1; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataId", "physicsClientId", NULL}; + static char* kwlist[] = {"userDataId", "physicsClientId", NULL}; 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; } @@ -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); return Py_None; @@ -837,15 +835,14 @@ static PyObject* pybullet_getNumUserData(PyObject* self, PyObject* args, PyObjec b3PhysicsClientHandle sm = 0; int physicsClientId = 0; int bodyUniqueId = -1; - int linkIndex = -1; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL}; + static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL}; 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; } @@ -856,7 +853,7 @@ static PyObject* pybullet_getNumUserData(PyObject* self, PyObject* args, PyObjec return NULL; } - numUserData = b3GetNumUserData(sm, bodyUniqueId, linkIndex); + numUserData = b3GetNumUserData(sm, bodyUniqueId); return PyInt_FromLong(numUserData); } @@ -865,16 +862,17 @@ static PyObject* pybullet_getUserDataInfo(PyObject* self, PyObject* args, PyObje b3PhysicsClientHandle sm = 0; int physicsClientId = 0; int bodyUniqueId = -1; - int linkIndex = -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; 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; } @@ -885,16 +883,19 @@ static PyObject* pybullet_getUserDataInfo(PyObject* self, PyObject* args, PyObje return NULL; } - b3GetUserDataInfo(sm, bodyUniqueId, linkIndex, userDataIndex, &key, &userDataId); + b3GetUserDataInfo(sm, bodyUniqueId, userDataIndex, &key, &userDataId, &linkIndex, &visualShapeIndex); if (key == 0 || userDataId == -1) { PyErr_SetString(SpamError, "Could not get user data info."); return NULL; } { - PyObject *userDataInfoTuple = PyTuple_New(2); + PyObject *userDataInfoTuple = PyTuple_New(5); PyTuple_SetItem(userDataInfoTuple, 0, PyInt_FromLong(userDataId)); 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; } } @@ -9131,28 +9132,28 @@ static PyMethodDef SpamMethods[] = { "Update user data, in case other clients made changes."}, {"addUserData", (PyCFunction)pybullet_addUserData, METH_VARARGS | METH_KEYWORDS, - "addUserData(bodyUniqueId, linkIndex, key, value, physicsClientId=0)\n" - "Adds or updates a user data entry to a link. Returns user data identifier."}, + "addUserData(bodyUniqueId, key, value, linkIndex=-1, visualShapeIndex=-1, physicsClientId=0)\n" + "Adds or updates a user data entry. Returns user data identifier."}, {"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."}, {"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."}, {"getUserDataId", (PyCFunction)pybullet_getUserDataId, METH_VARARGS | METH_KEYWORDS, - "getUserDataId(bodyUniqueId, linkIndex, key, physicsClientId=0)\n" - "Retrieves the userDataId on a link given the key."}, + "getUserDataId(bodyUniqueId, key, linkIndex=-1, visualShapeIndex=-1, physicsClientId=0)\n" + "Retrieves the userDataId given the key and optionally link and visual shape index."}, {"getNumUserData", (PyCFunction)pybullet_getNumUserData, METH_VARARGS | METH_KEYWORDS, - "getNumUserData(bodyUniqueId, linkIndex, physicsClientId=0)\n" - "Retrieves the number of user data entries in a link."}, + "getNumUserData(bodyUniqueId physicsClientId=0)\n" + "Retrieves the number of user data entries in a body."}, {"getUserDataInfo", (PyCFunction)pybullet_getUserDataInfo, METH_VARARGS | METH_KEYWORDS, - "getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, physicsClientId=0)\n" - "Retrieves the key and the identifier of a user data as (id, key)."}, + "getUserDataInfo(bodyUniqueId, userDataIndex, physicsClientId=0)\n" + "Retrieves the key and the identifier of a user data as (userDataId, key, bodyUniqueId, linkIndex, visualShapeIndex)."}, {"removeBody", (PyCFunction)pybullet_removeBody, METH_VARARGS | METH_KEYWORDS, "Remove a body by its body unique id."}, diff --git a/examples/pybullet/unittests/userDataTest.py b/examples/pybullet/unittests/userDataTest.py index 2b3db2cb4..e16b56958 100644 --- a/examples/pybullet/unittests/userDataTest.py +++ b/examples/pybullet/unittests/userDataTest.py @@ -26,169 +26,170 @@ class TestUserDataMethods(unittest.TestCase): def testAddUserData(self): plane_id = self.client.loadURDF(PLANE_PATH) - uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") - uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") - uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") - uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") + uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1") + uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2") + uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3") + uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4") # Retrieve user data and make sure it's correct. - self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1)) - self.assertEqual(b"MyValue2", self.client.getUserData(plane_id, 0, uid2)) - self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3)) - self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4)) + self.assertEqual(b"MyValue1", self.client.getUserData(uid1)) + self.assertEqual(b"MyValue2", self.client.getUserData(uid2)) + self.assertEqual(b"MyValue3", self.client.getUserData(uid3)) + self.assertEqual(b"MyValue4", self.client.getUserData(uid4)) # Disconnect/reconnect and make sure that the user data is synced back. del self.client self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) - self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1)) - self.assertEqual(b"MyValue2", self.client.getUserData(plane_id, 0, uid2)) - self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3)) - self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4)) + self.assertEqual(b"MyValue1", self.client.getUserData(uid1)) + self.assertEqual(b"MyValue2", self.client.getUserData(uid2)) + self.assertEqual(b"MyValue3", self.client.getUserData(uid3)) + self.assertEqual(b"MyValue4", self.client.getUserData(uid4)) self.client.resetSimulation() - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4)) + self.assertEqual(None, self.client.getUserData(uid1)) + self.assertEqual(None, self.client.getUserData(uid2)) + self.assertEqual(None, self.client.getUserData(uid3)) + self.assertEqual(None, self.client.getUserData(uid4)) def testGetNumUserData(self): plane_id = self.client.loadURDF(PLANE_PATH) - uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") - uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") - uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") - uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") + uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1") + uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2") + uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3") + 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 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): 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(b"MyNewValue", self.client.getUserData(plane_id, 0, uid)) + self.assertEqual(b"MyNewValue", self.client.getUserData(uid)) del self.client 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): plane_id = self.client.loadURDF(PLANE_PATH) - uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") - uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") - uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") - uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") + uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1") + uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2") + uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3") + uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4") - self.assertEqual(uid1, self.client.getUserDataId(plane_id, 0, "MyKey1")) - self.assertEqual(uid2, self.client.getUserDataId(plane_id, 0, "MyKey2")) - self.assertEqual(uid3, self.client.getUserDataId(plane_id, 0, "MyKey3")) - self.assertEqual(uid4, self.client.getUserDataId(plane_id, 0, "MyKey4")) + self.assertEqual(uid1, self.client.getUserDataId(plane_id, "MyKey1")) + self.assertEqual(uid2, self.client.getUserDataId(plane_id, "MyKey2")) + self.assertEqual(uid3, self.client.getUserDataId(plane_id, "MyKey3")) + self.assertEqual(uid4, self.client.getUserDataId(plane_id, "MyKey4")) del self.client self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) - self.assertEqual(uid1, self.client.getUserDataId(plane_id, 0, "MyKey1")) - self.assertEqual(uid2, self.client.getUserDataId(plane_id, 0, "MyKey2")) - self.assertEqual(uid3, self.client.getUserDataId(plane_id, 0, "MyKey3")) - self.assertEqual(uid4, self.client.getUserDataId(plane_id, 0, "MyKey4")) + self.assertEqual(uid1, self.client.getUserDataId(plane_id, "MyKey1")) + self.assertEqual(uid2, self.client.getUserDataId(plane_id, "MyKey2")) + self.assertEqual(uid3, self.client.getUserDataId(plane_id, "MyKey3")) + self.assertEqual(uid4, self.client.getUserDataId(plane_id, "MyKey4")) def testRemoveUserData(self): plane_id = self.client.loadURDF(PLANE_PATH) - uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") - uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") - uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") - uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") + uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1") + uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2") + uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3") + 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(-1, self.client.getUserDataId(plane_id, 0, "MyKey2")) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) - self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1)) - self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3)) - self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4)) + self.assertEqual(3, self.client.getNumUserData(plane_id)) + self.assertEqual(-1, self.client.getUserDataId(plane_id, "MyKey2")) + self.assertEqual(None, self.client.getUserData(uid2)) + self.assertEqual(b"MyValue1", self.client.getUserData(uid1)) + self.assertEqual(b"MyValue3", self.client.getUserData(uid3)) + self.assertEqual(b"MyValue4", self.client.getUserData(uid4)) del self.client self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) - self.assertEqual(3, self.client.getNumUserData(plane_id, 0)) - self.assertEqual(-1, self.client.getUserDataId(plane_id, 0, "MyKey2")) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) - self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1)) - self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3)) - self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4)) + self.assertEqual(3, self.client.getNumUserData(plane_id)) + self.assertEqual(-1, self.client.getUserDataId(plane_id, "MyKey2")) + self.assertEqual(None, self.client.getUserData(uid2)) + self.assertEqual(b"MyValue1", self.client.getUserData(uid1)) + self.assertEqual(b"MyValue3", self.client.getUserData(uid3)) + self.assertEqual(b"MyValue4", self.client.getUserData(uid4)) def testIterateAllUserData(self): plane_id = self.client.loadURDF(PLANE_PATH) - uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") - uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") - uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") - uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") + uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1") + uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2") + uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3") + uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4") entries = set() - for i in range(self.client.getNumUserData(plane_id, 0)): - userDataId, key = self.client.getUserDataInfo(plane_id, 0, i) - value = self.client.getUserData(plane_id, 0, userDataId); - entries.add((userDataId, key, value)) + for i in range(self.client.getNumUserData(plane_id)): + userDataId, key, bodyId, linkIndex, visualShapeIndex = self.client.getUserDataInfo(plane_id, i) + value = self.client.getUserData(userDataId); + entries.add((userDataId, key, value, bodyId, linkIndex, visualShapeIndex)) - self.assertTrue((uid1, b"MyKey1", b"MyValue1") in entries) - self.assertTrue((uid2, b"MyKey2", b"MyValue2") in entries) - self.assertTrue((uid3, b"MyKey3", b"MyValue3") in entries) - self.assertTrue((uid4, b"MyKey4", b"MyValue4") in entries) + self.assertTrue((uid1, b"MyKey1", b"MyValue1", plane_id, -1, -1) in entries) + self.assertTrue((uid2, b"MyKey2", b"MyValue2", plane_id, -1, -1) in entries) + self.assertTrue((uid3, b"MyKey3", b"MyValue3", plane_id, -1, -1) in entries) + self.assertTrue((uid4, b"MyKey4", b"MyValue4", plane_id, -1, -1) in entries) self.assertEqual(4, len(entries)) def testRemoveBody(self): plane_id = self.client.loadURDF(PLANE_PATH) - uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") - uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") - uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") - uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") + uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1") + uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2") + uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3") + uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4") self.client.removeBody(plane_id) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4)) + self.assertEqual(None, self.client.getUserData(uid1)) + self.assertEqual(None, self.client.getUserData(uid2)) + self.assertEqual(None, self.client.getUserData(uid3)) + self.assertEqual(None, self.client.getUserData(uid4)) del self.client self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4)) + self.assertEqual(None, self.client.getUserData(uid1)) + self.assertEqual(None, self.client.getUserData(uid2)) + self.assertEqual(None, self.client.getUserData(uid3)) + self.assertEqual(None, self.client.getUserData(uid4)) + def testMultipleBodies(self): plane1 = self.client.loadURDF(PLANE_PATH) plane2 = self.client.loadURDF(PLANE_PATH) - uid1 = self.client.addUserData(plane1, 0, "MyKey1", "This is plane 1 - 1") - uid2 = self.client.addUserData(plane1, 0, "MyKey2", "This is plane 1 - 2") + uid1 = self.client.addUserData(plane1, "MyKey1", "This is plane 1 - 1") + uid2 = self.client.addUserData(plane1, "MyKey2", "This is plane 1 - 2") - uid3 = self.client.addUserData(plane2, 0, "MyKey1", "This is plane 2 - 1") - uid4 = self.client.addUserData(plane2, 0, "MyKey2", "This is plane 2 - 2") - uid5 = self.client.addUserData(plane2, 0, "MyKey3", "This is plane 2 - 3") + uid3 = self.client.addUserData(plane2, "MyKey1", "This is plane 2 - 1") + uid4 = self.client.addUserData(plane2, "MyKey2", "This is plane 2 - 2") + 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 - 2", self.client.getUserData(plane1, 0, self.client.getUserDataId(plane1, 0, "MyKey2"))) + 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(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 - 2", self.client.getUserData(plane2, 0, self.client.getUserDataId(plane2, 0, "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 - 1", self.client.getUserData(self.client.getUserDataId(plane2, "MyKey1"))) + 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(self.client.getUserDataId(plane2, "MyKey3"))) def testMultipleLinks(self): @@ -198,14 +199,15 @@ class TestUserDataMethods(unittest.TestCase): self.assertTrue(num_links > 1) for link_index in range(num_links): - uid1 = self.client.addUserData(body_id, link_index, "MyKey1", "Value1 for link %s" % link_index) - uid2 = self.client.addUserData(body_id, link_index, "MyKey2", "Value2 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, "MyKey2", "Value2 for link %s" % link_index, link_index) for link_index in range(num_links): - uid1 = self.client.getUserDataId(body_id, link_index, "MyKey1") - uid2 = self.client.getUserDataId(body_id, link_index, "MyKey2") - self.assertEqual(("Value1 for link %s" % link_index).encode(), self.client.getUserData(body_id, link_index, uid1)) - self.assertEqual(("Value2 for link %s" % link_index).encode(), self.client.getUserData(body_id, link_index, uid2)) + uid1 = self.client.getUserDataId(body_id, "MyKey1", link_index) + uid2 = self.client.getUserDataId(body_id, "MyKey2", link_index) + 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(uid2)) + def testMultipleClients(self): client1 = self.client @@ -215,25 +217,48 @@ class TestUserDataMethods(unittest.TestCase): client2.syncBodyInfo() # Add user data on client 1, check on client 1 - uid = client1.addUserData(plane_id, 0, "MyKey", "MyValue") - self.assertEqual(None, client2.getUserData(plane_id, 0, uid)) + uid = client1.addUserData(plane_id, "MyKey", "MyValue") + self.assertEqual(None, client2.getUserData(uid)) 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 - client2.addUserData(plane_id, 0, "MyKey", "MyNewValue") - self.assertEqual(b"MyValue", client1.getUserData(plane_id, 0, uid)) + client2.addUserData(plane_id, "MyKey", "MyNewValue") + self.assertEqual(b"MyValue", client1.getUserData(uid)) 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 - client1.removeUserData(plane_id, 0, uid) - self.assertEqual(b"MyNewValue", client2.getUserData(plane_id, 0, uid)) + client1.removeUserData(uid) + self.assertEqual(b"MyNewValue", client2.getUserData(uid)) client2.syncUserData() - self.assertEqual(None, client2.getUserData(plane_id, 0, uid)) + self.assertEqual(None, client2.getUserData(uid)) 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__": unittest.main()