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 de467657d..ab0314a1a 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; @@ -5089,34 +5020,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; } @@ -5127,21 +5035,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; @@ -5164,27 +5067,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); @@ -5199,19 +5118,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; @@ -8325,6 +8245,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); } @@ -10561,6 +10487,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..e92333b43 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 b96b0fbfe..9ad100bdd 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -7,7 +7,8 @@ //Please don't replace an existing magic number: //instead, only ADD a new one at the top, comment-out previous one -#define SHARED_MEMORY_MAGIC_NUMBER 201806150 +#define SHARED_MEMORY_MAGIC_NUMBER 201807040 +//#define SHARED_MEMORY_MAGIC_NUMBER 201806150 //#define SHARED_MEMORY_MAGIC_NUMBER 201806020 //#define SHARED_MEMORY_MAGIC_NUMBER 201801170 //#define SHARED_MEMORY_MAGIC_NUMBER 201801080 @@ -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..aabc48bff 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,43 @@ struct SharedMemoryUserData } }; +struct SharedMemoryUserDataHashKey { + unsigned int m_hash; + + btHashString m_key; + btHashInt m_bodyUniqueId; + btHashInt m_linkIndex; + btHashInt m_visualShapeIndex; + + 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 5d5ceb79f..836f06312 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -713,18 +713,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; } @@ -736,7 +737,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); @@ -754,16 +755,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; } @@ -774,7 +773,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); @@ -794,15 +793,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; } @@ -813,7 +813,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); } @@ -821,16 +821,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; } @@ -842,7 +840,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; @@ -861,15 +859,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; } @@ -880,7 +877,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); } @@ -889,16 +886,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; } @@ -909,16 +907,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; } } @@ -9164,28 +9165,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()