From 9c7aa3a863447c3dd323515266576ccb05ae963f Mon Sep 17 00:00:00 2001 From: Tigran Gasparian Date: Tue, 3 Jul 2018 17:45:19 +0200 Subject: [PATCH 01/14] 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() From 7c7e8af717a3f3ca587810f8d71e04671c3ed8e9 Mon Sep 17 00:00:00 2001 From: Tigran Gasparian Date: Wed, 4 Jul 2018 10:22:44 +0200 Subject: [PATCH 02/14] Increases the shared memory magic number. --- examples/SharedMemory/SharedMemoryPublic.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 86d55db3c..1e4ba1f72 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 From 8a6db042defbe27d1281edc6f2ac6e1817940d09 Mon Sep 17 00:00:00 2001 From: Tigran Gasparian Date: Wed, 4 Jul 2018 14:25:48 +0200 Subject: [PATCH 03/14] Removes int initializer in SharedMemoryUserDataHashKey, changes '>>' into '> >' for nexted templates. --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- examples/SharedMemory/SharedMemoryUserData.h | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 502d757f5..6284451d1 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1538,7 +1538,7 @@ struct PhysicsServerCommandProcessorInternalData b3ResizablePool< InternalBodyHandle > m_bodyHandles; b3ResizablePool m_userCollisionShapeHandles; b3ResizablePool m_userVisualShapeHandles; - b3ResizablePool> m_userDataHandles; + b3ResizablePool > m_userDataHandles; btHashMap m_userDataHandleLookup; b3PluginManager m_pluginManager; diff --git a/examples/SharedMemory/SharedMemoryUserData.h b/examples/SharedMemory/SharedMemoryUserData.h index 991714557..aabc48bff 100644 --- a/examples/SharedMemory/SharedMemoryUserData.h +++ b/examples/SharedMemory/SharedMemoryUserData.h @@ -48,15 +48,14 @@ struct SharedMemoryUserData } }; -class SharedMemoryUserDataHashKey { - unsigned int m_hash = 0; +struct SharedMemoryUserDataHashKey { + unsigned int m_hash; btHashString m_key; btHashInt m_bodyUniqueId; btHashInt m_linkIndex; btHashInt m_visualShapeIndex; -public: SIMD_FORCE_INLINE unsigned int getHash()const { return m_hash; } From 49684144dc18deedfeefe5557aaf26c0bca52c3b Mon Sep 17 00:00:00 2001 From: Tigran Gasparian Date: Thu, 5 Jul 2018 16:01:03 +0200 Subject: [PATCH 04/14] Some minor formatting fixes --- examples/SharedMemory/SharedMemoryCommands.h | 8 ++++---- examples/SharedMemory/SharedMemoryPublic.h | 1 - 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index e529f66b1..e92333b43 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -1004,9 +1004,9 @@ struct UserDataRequestArgs { struct UserDataResponseArgs { int m_userDataId; - int m_bodyUniqueId; - int m_linkIndex; - int m_visualShapeIndex; + int m_bodyUniqueId; + int m_linkIndex; + int m_visualShapeIndex; int m_valueType; int m_valueLength; char m_key[MAX_USER_DATA_KEY_LENGTH]; @@ -1017,7 +1017,7 @@ struct AddUserDataRequestArgs { int m_bodyUniqueId; int m_linkIndex; - int m_visualShapeIndex; + int m_visualShapeIndex; int m_valueType; int m_valueLength; char m_key[MAX_USER_DATA_KEY_LENGTH]; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 1e4ba1f72..0ad8b864f 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -275,7 +275,6 @@ 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 From 6dbdf0280848b521e31cdc337c618d82a805ae5b Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 9 Jul 2018 20:54:13 -0700 Subject: [PATCH 05/14] Minor code optimization in multibody forward dynamics --- .../Featherstone/btMultiBody.cpp | 35 +++++++++---------- 1 file changed, 16 insertions(+), 19 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 0493cdb02..b9eabef64 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -744,8 +744,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar const btScalar DAMPING_K1_ANGULAR = m_angularDamping; const btScalar DAMPING_K2_ANGULAR= m_angularDamping; - btVector3 base_vel = getBaseVel(); - btVector3 base_omega = getBaseOmega(); + const btVector3 base_vel = getBaseVel(); + const btVector3 base_omega = getBaseOmega(); // Temporary matrices/vectors -- use scratch space from caller // so that we don't have to keep reallocating every frame @@ -781,7 +781,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // hhat is NOT stored for the base (but ahat is) btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; - v_ptr += num_links * 2 + 2; + // v_ptr += num_links * 2 + 2; // Disabled since v_ptr is not used in the rest of the code // // Y_i, invD_i btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; @@ -819,13 +819,13 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar } else { - btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce; - btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque; + const btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce; + const btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque; //external forces zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce)); //adding damping terms (only) - btScalar linDampMult = 1., angDampMult = 1.; + const btScalar linDampMult = 1., angDampMult = 1.; zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()), linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm())); @@ -969,14 +969,11 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) - spatCoriolisAcc[i].dot(hDof) ; - } - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - btScalar *D_row = &D[dof * m_links[i].m_dofCount]; + btScalar *D_row = &D[dof * m_links[i].m_dofCount]; for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) { - btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; + const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2); } } @@ -999,8 +996,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar case btMultibodyLink::eSpherical: case btMultibodyLink::ePlanar: { - btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); - btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse(); + const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); + const btMatrix3x3 invD3x3(D3x3.inverse()); //unroll the loop? for(int row = 0; row < 3; ++row) @@ -1026,7 +1023,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) { - btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; + const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; // spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof]; } @@ -1037,7 +1034,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar //determine (h*D^{-1}) * h^{T} for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) { - btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; // dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]); } @@ -1058,7 +1055,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) { - btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; // spatForceVecTemps[0] += hDof * invD_times_Y[dof]; } @@ -1109,7 +1106,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) { - btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; // Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof); } @@ -1169,12 +1166,12 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar } // transform base accelerations back to the world frame. - btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); + const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); output[0] = omegadot_out[0]; output[1] = omegadot_out[1]; output[2] = omegadot_out[2]; - btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear())); + const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear())); output[3] = vdot_out[0]; output[4] = vdot_out[1]; output[5] = vdot_out[2]; From deea2bb411c54fd7f8116c848f28c32e31aef4af Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 9 Jul 2018 20:59:29 -0700 Subject: [PATCH 06/14] Use const reference for referencing existing members --- src/BulletDynamics/Featherstone/btMultiBody.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index b9eabef64..8023f63c0 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -819,8 +819,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar } else { - const btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce; - const btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque; + const btVector3& baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce; + const btVector3& baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque; //external forces zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce)); From 3d1cd749b497e84d04a024067c457a5911989c4b Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 9 Jul 2018 21:57:57 -0700 Subject: [PATCH 07/14] Add Travis CI settings for Xenial and Bionic --- .ci/docker/env.list | 7 ++++ .ci/docker/ubuntu-bionic | 18 +++++++++ .ci/docker/ubuntu-xenial | 17 ++++++++ .ci/script.sh | 29 ++++++++++++++ .travis.yml | 87 +++++++++++++++++++++++++--------------- CMakeLists.txt | 2 +- 6 files changed, 126 insertions(+), 34 deletions(-) create mode 100644 .ci/docker/env.list create mode 100644 .ci/docker/ubuntu-bionic create mode 100644 .ci/docker/ubuntu-xenial create mode 100755 .ci/script.sh diff --git a/.ci/docker/env.list b/.ci/docker/env.list new file mode 100644 index 000000000..77dcd5baa --- /dev/null +++ b/.ci/docker/env.list @@ -0,0 +1,7 @@ +TRAVIS_OS_NAME +TRAVIS_PULL_REQUEST + +BUILD_NAME +CC +CXX +SUDO diff --git a/.ci/docker/ubuntu-bionic b/.ci/docker/ubuntu-bionic new file mode 100644 index 000000000..b7643438e --- /dev/null +++ b/.ci/docker/ubuntu-bionic @@ -0,0 +1,18 @@ +FROM ubuntu:bionic + +RUN apt-get update -qq +RUN apt-get install -y \ + build-essential \ + clang \ + cmake \ + curl \ + git \ + libgl-dev \ + libglu-dev \ + libpython3-dev \ + lsb-release \ + pkg-config \ + python3 \ + python3-distutils \ + software-properties-common \ + sudo diff --git a/.ci/docker/ubuntu-xenial b/.ci/docker/ubuntu-xenial new file mode 100644 index 000000000..693e8a1fd --- /dev/null +++ b/.ci/docker/ubuntu-xenial @@ -0,0 +1,17 @@ +FROM ubuntu:xenial + +RUN apt-get update -qq +RUN apt-get install -y \ + build-essential \ + clang \ + cmake \ + curl \ + git \ + libgl-dev \ + libglu-dev \ + libpython3-dev \ + lsb-release \ + pkg-config \ + python3 \ + software-properties-common \ + sudo diff --git a/.ci/script.sh b/.ci/script.sh new file mode 100755 index 000000000..0e67bf9f2 --- /dev/null +++ b/.ci/script.sh @@ -0,0 +1,29 @@ +#!/usr/bin/env bash +set -ex + +echo "CXX="$CXX +echo "CC="$CC +if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then + $SUDO apt-get install -y python3 + $SUDO apt-get install -y python3-pip + $SUDO pip3 install -U wheel + $SUDO pip3 install -U setuptools + $SUDO python3 setup.py install + python3 examples/pybullet/unittests/unittests.py --verbose + python3 examples/pybullet/unittests/userDataTest.py --verbose + python3 examples/pybullet/unittests/saveRestoreStateTest.py --verbose +fi +cmake . -DBUILD_PYBULLET=ON -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror +make -j8 +ctest -j8 --output-on-failure + +# Build again with double precision +cmake . -G "Unix Makefiles" -DUSE_DOUBLE_PRECISION=ON #-DCMAKE_CXX_FLAGS=-Werror +make -j8 +ctest -j8 --output-on-failure + +# Build again with shared libraries +cmake . -G "Unix Makefiles" -DBUILD_SHARED_LIBS=ON +make -j8 +ctest -j8 --output-on-failure +$SUDO make install diff --git a/.travis.yml b/.travis.yml index c93c1d2ce..fbaf6d90f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,35 +1,56 @@ language: cpp -os: - - linux - - osx -compiler: - - gcc - - clang -addons: - apt: - packages: - - python3 - - +matrix: + include: + - os: linux + compiler: gcc + env: + - BUILD_NAME=TRUSTY_GCC + - SUDO=sudo + - os: linux + compiler: clang + env: + - BUILD_NAME=TRUSTY_CLANG + - SUDO=sudo + - os: linux + compiler: gcc + env: + - BUILD_NAME=XENIAL_GCC + - DOCKER_FILE="ubuntu-xenial" + services: docker + - os: linux + compiler: clang + env: + - BUILD_NAME=XENIAL_CLANG + - DOCKER_FILE="ubuntu-xenial" + services: docker + - os: linux + compiler: gcc + env: + - BUILD_NAME=BIONIC_GCC + - DOCKER_FILE="ubuntu-bionic" + services: docker + - os: linux + compiler: clang + env: + - BUILD_NAME=BIONIC_CLANG + - DOCKER_FILE="ubuntu-bionic" + services: docker + - os: osx + compiler: gcc + env: + - BUILD_NAME=OSX_GCC + - os: osx + compiler: clang + env: + - BUILD_NAME=OSX_CLANG +before_install: + - if [ -n "$DOCKER_FILE" ]; then + docker build -t "$DOCKER_FILE" -f ".ci/docker/$DOCKER_FILE" .; + docker run -itd -v $TRAVIS_BUILD_DIR:$TRAVIS_BUILD_DIR --env-file .ci/docker/env.list --name bullet-docker "$DOCKER_FILE"; + fi script: - - echo "CXX="$CXX - - echo "CC="$CC - - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then sudo apt-get install python3-pip; fi - - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then sudo pip3 install -U pip wheel; fi - - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then sudo pip3 install setuptools; fi - - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then sudo python3 setup.py install; fi - - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then python3 examples/pybullet/unittests/unittests.py --verbose; fi - - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then python3 examples/pybullet/unittests/userDataTest.py --verbose; fi - - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then python3 examples/pybullet/unittests/saveRestoreStateTest.py --verbose; fi - - cmake . -DBUILD_PYBULLET=ON -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror - - make -j8 - - ctest -j8 --output-on-failure - # Build again with double precision - - cmake . -G "Unix Makefiles" -DUSE_DOUBLE_PRECISION=ON #-DCMAKE_CXX_FLAGS=-Werror - - make -j8 - - ctest -j8 --output-on-failure - # Build again with shared libraries - - cmake . -G "Unix Makefiles" -DBUILD_SHARED_LIBS=ON - - make -j8 - - ctest -j8 --output-on-failure - - sudo make install + - if [ -n "$DOCKER_FILE" ]; then + docker exec bullet-docker /bin/sh -c "cd $TRAVIS_BUILD_DIR && ./.ci/script.sh"; + else + '.ci/script.sh'; + fi diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a951d25a..728ac7608 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -316,7 +316,7 @@ IF(BUILD_PYBULLET) SET(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/build3/cmake ${CMAKE_MODULE_PATH}) OPTION(EXACT_PYTHON_VERSION "Require Python and match PYTHON_VERSION_PYBULLET exactly, e.g. 2.7.12" OFF) IF(EXACT_PYTHON_VERSION) - set(EXACT_PYTHON_VERSION_FLAG EXACT REQUIRED) + set(EXACT_PYTHON_VERSION_FLAG EXACT REQUIRED) ENDIF(EXACT_PYTHON_VERSION) # first find the python interpreter FIND_PACKAGE(PythonInterp ${PYTHON_VERSION_PYBULLET} ${EXACT_PYTHON_VERSION_FLAG}) From 64db9af036d20563317d0c4e9c06bf01785c15cd Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 10 Jul 2018 09:20:00 -0700 Subject: [PATCH 08/14] Fix c++11-narrowing errors in multibody examples --- examples/MultiBody/InvertedPendulumPDControl.cpp | 2 +- examples/MultiBody/MultiBodyConstraintFeedback.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index d380929c4..f6277739b 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -268,7 +268,7 @@ btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GU btVector3 posr = local_origin[i+1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + const btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; btCollisionShape* shape =0; if (i==0) diff --git a/examples/MultiBody/MultiBodyConstraintFeedback.cpp b/examples/MultiBody/MultiBodyConstraintFeedback.cpp index c08d98f72..f4d959358 100644 --- a/examples/MultiBody/MultiBodyConstraintFeedback.cpp +++ b/examples/MultiBody/MultiBodyConstraintFeedback.cpp @@ -320,7 +320,7 @@ void MultiBodyConstraintFeedbackSetup::initPhysics() btVector3 posr = local_origin[i+1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + const btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; btCollisionShape* shape =0; if (i==0) From 6f27baf39264e30ed7aea9cdb02d33ca1bb1824f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 10 Jul 2018 09:21:49 -0700 Subject: [PATCH 09/14] Revert "Fix c++11-narrowing errors in multibody examples" This reverts commit 64db9af036d20563317d0c4e9c06bf01785c15cd. --- examples/MultiBody/InvertedPendulumPDControl.cpp | 2 +- examples/MultiBody/MultiBodyConstraintFeedback.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index f6277739b..d380929c4 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -268,7 +268,7 @@ btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GU btVector3 posr = local_origin[i+1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - const btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; btCollisionShape* shape =0; if (i==0) diff --git a/examples/MultiBody/MultiBodyConstraintFeedback.cpp b/examples/MultiBody/MultiBodyConstraintFeedback.cpp index f4d959358..c08d98f72 100644 --- a/examples/MultiBody/MultiBodyConstraintFeedback.cpp +++ b/examples/MultiBody/MultiBodyConstraintFeedback.cpp @@ -320,7 +320,7 @@ void MultiBodyConstraintFeedbackSetup::initPhysics() btVector3 posr = local_origin[i+1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - const btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; btCollisionShape* shape =0; if (i==0) From 6323309f892a64ddbb220bdceeb6345702210bf6 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 10 Jul 2018 09:20:00 -0700 Subject: [PATCH 10/14] Fix c++11-narrowing errors in multibody examples --- examples/MultiBody/InvertedPendulumPDControl.cpp | 2 +- examples/MultiBody/MultiBodyConstraintFeedback.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index d380929c4..f6277739b 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -268,7 +268,7 @@ btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GU btVector3 posr = local_origin[i+1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + const btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; btCollisionShape* shape =0; if (i==0) diff --git a/examples/MultiBody/MultiBodyConstraintFeedback.cpp b/examples/MultiBody/MultiBodyConstraintFeedback.cpp index c08d98f72..f4d959358 100644 --- a/examples/MultiBody/MultiBodyConstraintFeedback.cpp +++ b/examples/MultiBody/MultiBodyConstraintFeedback.cpp @@ -320,7 +320,7 @@ void MultiBodyConstraintFeedbackSetup::initPhysics() btVector3 posr = local_origin[i+1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + const btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; btCollisionShape* shape =0; if (i==0) From b3d863900496a8c5dc150adfa8ab21c48bbdd97c Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 10 Jul 2018 11:26:00 -0700 Subject: [PATCH 11/14] Run apt-get update before installing python3-pip --- .ci/script.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/.ci/script.sh b/.ci/script.sh index 0e67bf9f2..7fff323bf 100755 --- a/.ci/script.sh +++ b/.ci/script.sh @@ -4,6 +4,7 @@ set -ex echo "CXX="$CXX echo "CC="$CC if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then + $SUDO apt-get update $SUDO apt-get install -y python3 $SUDO apt-get install -y python3-pip $SUDO pip3 install -U wheel From 8857638dedce4c10b68baf72909dd99fd9dc9821 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 18 Jul 2018 11:57:34 -0700 Subject: [PATCH 12/14] Revert C++ code changes to make the PR small --- examples/MultiBody/InvertedPendulumPDControl.cpp | 2 +- examples/MultiBody/MultiBodyConstraintFeedback.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index f6277739b..d380929c4 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -268,7 +268,7 @@ btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GU btVector3 posr = local_origin[i+1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - const btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; btCollisionShape* shape =0; if (i==0) diff --git a/examples/MultiBody/MultiBodyConstraintFeedback.cpp b/examples/MultiBody/MultiBodyConstraintFeedback.cpp index f4d959358..c08d98f72 100644 --- a/examples/MultiBody/MultiBodyConstraintFeedback.cpp +++ b/examples/MultiBody/MultiBodyConstraintFeedback.cpp @@ -320,7 +320,7 @@ void MultiBodyConstraintFeedbackSetup::initPhysics() btVector3 posr = local_origin[i+1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - const btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; btCollisionShape* shape =0; if (i==0) From 8b26945f8f4f098d86984c36d6cf71adfce86a1b Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 18 Jul 2018 12:02:36 -0700 Subject: [PATCH 13/14] Fix build errors with Clang 6 (also addresses #1510) --- examples/MultiBody/InvertedPendulumPDControl.cpp | 2 +- examples/MultiBody/MultiBodyConstraintFeedback.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index d380929c4..f6277739b 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -268,7 +268,7 @@ btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GU btVector3 posr = local_origin[i+1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + const btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; btCollisionShape* shape =0; if (i==0) diff --git a/examples/MultiBody/MultiBodyConstraintFeedback.cpp b/examples/MultiBody/MultiBodyConstraintFeedback.cpp index c08d98f72..f4d959358 100644 --- a/examples/MultiBody/MultiBodyConstraintFeedback.cpp +++ b/examples/MultiBody/MultiBodyConstraintFeedback.cpp @@ -320,7 +320,7 @@ void MultiBodyConstraintFeedbackSetup::initPhysics() btVector3 posr = local_origin[i+1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + const btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; btCollisionShape* shape =0; if (i==0) From 7a27cb1739a4dcc83f454090bfcd1333ae7e9a97 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 22 Jul 2018 13:03:53 +0200 Subject: [PATCH 14/14] body1 -> bodyA and body2 -> bodyB --- .../btSequentialImpulseConstraintSolver.cpp | 160 +++++++++--------- 1 file changed, 80 insertions(+), 80 deletions(-) diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index c03f9f861..32d77a737 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -43,11 +43,11 @@ int gNumSplitImpulseRecoveries = 0; //#define VERBOSE_RESIDUAL_PRINTF 1 ///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver ///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check. -static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity()); // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; @@ -69,18 +69,18 @@ static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody c.m_appliedImpulse = sum; } - body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); - body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + bodyA.internalApplyImpulse(c.m_contactNormal1*bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + bodyB.internalApplyImpulse(c.m_contactNormal2*bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); return deltaImpulse*(1./c.m_jacDiagABInv); } -static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity()); deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; @@ -94,8 +94,8 @@ static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverB { c.m_appliedImpulse = sum; } - body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); - body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + bodyA.internalApplyImpulse(c.m_contactNormal1*bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + bodyB.internalApplyImpulse(c.m_contactNormal2*bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); return deltaImpulse*(1./c.m_jacDiagABInv); } @@ -150,14 +150,14 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) #endif // Project Gauss Seidel or the equivalent Sequential Impulse -static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); @@ -170,27 +170,27 @@ static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btS __m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp); deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied)); c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1)); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, body2.internalGetInvMass().mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, bodyB.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); - body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); - body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); - body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); + bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); + bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); + bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); + bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); return deltaImpulse.m_floats[0]/c.m_jacDiagABInv; } // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3 -static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { #if defined (BT_ALLOW_SSE4) __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm); const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit); - const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); - const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); + const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); + const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum @@ -198,27 +198,27 @@ static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& bod const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp); deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower); c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower); - body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128); - body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128); - body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128); - body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128); + bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128); + bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128); + bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128); + bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128); btSimdScalar deltaImp = deltaImpulse; return deltaImp.m_floats[0]*(1./c.m_jacDiagABInv); #else - return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c); + return gResolveSingleConstraintRowGeneric_sse2(bodyA,bodyB,c); #endif } -static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); @@ -228,40 +228,40 @@ static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp); deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse)); c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum)); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); - body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); - body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); - body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); + bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); + bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); + bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); + bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); return deltaImpulse.m_floats[0]/c.m_jacDiagABInv; } // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3 -static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { #ifdef BT_ALLOW_SSE4 __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm); const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); - const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); - const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); + const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); + const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit); deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask); c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask); - body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128); - body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128); - body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128); - body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128); + bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128); + bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128); + bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128); + bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128); btSimdScalar deltaImp = deltaImpulse; return deltaImp.m_floats[0]*(1./c.m_jacDiagABInv); #else - return gResolveSingleConstraintRowLowerLimit_sse2(body1,body2,c); + return gResolveSingleConstraintRowLowerLimit_sse2(bodyA,bodyB,c); #endif //BT_ALLOW_SSE4 } @@ -270,32 +270,32 @@ static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& -btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) { - return m_resolveSingleConstraintRowGeneric(body1, body2, c); + return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c); } // Project Gauss Seidel or the equivalent Sequential Impulse -btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) { - return m_resolveSingleConstraintRowGeneric(body1, body2, c); + return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c); } -btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) { - return m_resolveSingleConstraintRowLowerLimit(body1, body2, c); + return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c); } -btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) { - return m_resolveSingleConstraintRowLowerLimit(body1, body2, c); + return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c); } static btScalar gResolveSplitPenetrationImpulse_scalar_reference( - btSolverBody& body1, - btSolverBody& body2, + btSolverBody& bodyA, + btSolverBody& bodyB, const btSolverConstraint& c) { btScalar deltaImpulse = 0.f; @@ -304,8 +304,8 @@ static btScalar gResolveSplitPenetrationImpulse_scalar_reference( { gNumSplitImpulseRecoveries++; deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); - const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetTurnVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetTurnVelocity()); deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; @@ -319,13 +319,13 @@ static btScalar gResolveSplitPenetrationImpulse_scalar_reference( { c.m_appliedPushImpulse = sum; } - body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + bodyA.internalApplyPushImpulse(c.m_contactNormal1*bodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + bodyB.internalApplyPushImpulse(c.m_contactNormal2*bodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); } return deltaImpulse*(1./c.m_jacDiagABInv); } -static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) { #ifdef USE_SIMD if (!c.m_rhsPenetration) @@ -337,8 +337,8 @@ static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btSolve __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,bodyA.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,bodyA.internalGetTurnVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,bodyB.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,bodyB.internalGetTurnVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); @@ -348,17 +348,17 @@ static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btSolve __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,bodyA.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,bodyB.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); + bodyA.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyA.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + bodyA.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyA.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + bodyB.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyB.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + bodyB.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyB.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); btSimdScalar deltaImp = deltaImpulse; return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv); #else - return gResolveSplitPenetrationImpulse_scalar_reference(body1,body2,c); + return gResolveSplitPenetrationImpulse_scalar_reference(bodyA,bodyB,c); #endif } @@ -552,7 +552,7 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; - btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; + btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -576,12 +576,12 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr solverConstraint.m_angularComponentA .setZero(); } - if (body1) + if (bodyA) { solverConstraint.m_contactNormal2 = -normalAxis; btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2); solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor(); + solverConstraint.m_angularComponentB = bodyA->getInvInertiaTensorWorld()*ftorqueAxis1*bodyA->getAngularFactor(); } else { solverConstraint.m_contactNormal2.setZero(); @@ -598,10 +598,10 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); denom0 = body0->getInvMass() + normalAxis.dot(vec); } - if (body1) + if (bodyA) { vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = body1->getInvMass() + normalAxis.dot(vec); + denom1 = bodyA->getInvMass() + normalAxis.dot(vec); } btScalar denom = relaxation/(denom0+denom1); solverConstraint.m_jacDiagABInv = denom; @@ -613,8 +613,8 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr btScalar rel_vel; btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(bodyA?solverBodyB.m_angularVelocity:btVector3(0,0,0)); rel_vel = vel1Dotn+vel2Dotn; @@ -666,7 +666,7 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSo btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; - btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; + btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -685,13 +685,13 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSo { btVector3 ftorqueAxis1 = normalAxis1; solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_angularComponentB = bodyA ? bodyA->getInvInertiaTensorWorld()*ftorqueAxis1*bodyA->getAngularFactor() : btVector3(0,0,0); } { btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0); - btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0); + btVector3 iMJaB = bodyA?bodyA->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0); btScalar sum = 0; sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); @@ -704,8 +704,8 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSo btScalar rel_vel; btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(bodyA?solverBodyB.m_angularVelocity:btVector3(0,0,0)); rel_vel = vel1Dotn+vel2Dotn;