diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 785e01e7b..7c97996df 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -73,6 +73,12 @@ 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; }; + + #endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index afa1da448..0dc931a93 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1414,6 +1414,20 @@ B3_SHARED_API void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandH } } + +B3_SHARED_API void b3CreateMultiBodySetFlags(b3SharedMemoryCommandHandle commandHandle, int flags) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_MULTI_BODY); + if (command->m_type==CMD_CREATE_MULTI_BODY) + { + command->m_updateFlags |= MULT_BODY_HAS_FLAGS; + command->m_createMultiBodyArgs.m_flags = flags; + } + +} + B3_SHARED_API int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; @@ -2777,6 +2791,103 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsCli return (b3SharedMemoryCommandHandle) command; } +B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsClientHandle physClient) { + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_SYNC_USER_DATA; + return (b3SharedMemoryCommandHandle) command; +} + +B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char* key, UserDataValueType valueType, int valueLength, const void *valueData) { + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(strlen(key) < MAX_USER_DATA_KEY_LENGTH); + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + + command->m_type = CMD_ADD_USER_DATA; + command->m_addUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId; + command->m_addUserDataRequestArgs.m_linkIndex = linkIndex; + command->m_addUserDataRequestArgs.m_valueType = valueType; + command->m_addUserDataRequestArgs.m_valueLength = valueLength; + strcpy(command->m_addUserDataRequestArgs.m_key, key); + cl->uploadBulletFileToSharedMemory((const char *)valueData, valueLength); + + return (b3SharedMemoryCommandHandle) command; +} + +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId) { + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + 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) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + if (cl) + { + return cl->getCachedUserData(bodyUniqueId, linkIndex, userDataId, *valueOut); + } + return false; +} + +B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char *key) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + if (cl) + { + return cl->getCachedUserDataId(bodyUniqueId, linkIndex, key); + } + return -1; +} + +B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHandle) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + if (status) + { + btAssert(status->m_type == CMD_ADD_USER_DATA_COMPLETED); + return status->m_userDataResponseArgs.m_userDataGlobalId.m_userDataId; + } + return -1; +} + +B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + if (cl) + { + return cl->getNumUserData(bodyUniqueId, linkIndex); + } + return 0; +} + +B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + if (cl) + { + cl->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut); + } +} + + B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 3c23c2003..1776c1da6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -113,6 +113,17 @@ B3_SHARED_API int b3ComputeDofCount(b3PhysicsClientHandle physClient, int bodyUn ///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, struct b3JointInfo* info); +///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 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 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 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 B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info); @@ -466,6 +477,8 @@ B3_SHARED_API int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandl //useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet B3_SHARED_API void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle); +B3_SHARED_API void b3CreateMultiBodySetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); + //int b3CreateMultiBodyAddLink(b3SharedMemoryCommandHandle commandHandle, int jointType, int parentLinkIndex, double linkMass, int linkCollisionShapeUnique, int linkVisualShapeUniqueId); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index c78eff91c..0f2ec78c0 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -11,16 +11,34 @@ #include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" #include "SharedMemoryBlock.h" #include "BodyJointInfoUtility.h" +#include "SharedMemoryUserData.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; + + ~BodyJointInfoCache() + { + } }; struct PhysicsClientSharedMemoryInternalData { @@ -28,32 +46,34 @@ struct PhysicsClientSharedMemoryInternalData { bool m_ownsSharedMemory; SharedMemoryBlock* m_testBlock1; - b3HashMap m_bodyJointMap; - b3HashMap m_userConstraintInfoMap; + btHashMap m_bodyJointMap; + btHashMap m_userConstraintInfoMap; - b3AlignedObjectArray m_debugLinesFrom; - b3AlignedObjectArray m_debugLinesTo; - b3AlignedObjectArray m_debugLinesColor; + btAlignedObjectArray m_debugLinesFrom; + btAlignedObjectArray m_debugLinesTo; + btAlignedObjectArray m_debugLinesColor; int m_cachedCameraPixelsWidth; int m_cachedCameraPixelsHeight; - b3AlignedObjectArray m_cachedCameraPixelsRGBA; - b3AlignedObjectArray m_cachedCameraDepthBuffer; - b3AlignedObjectArray m_cachedSegmentationMaskBuffer; + btAlignedObjectArray m_cachedCameraPixelsRGBA; + btAlignedObjectArray m_cachedCameraDepthBuffer; + btAlignedObjectArray m_cachedSegmentationMaskBuffer; - b3AlignedObjectArray m_cachedContactPoints; - b3AlignedObjectArray m_cachedOverlappingObjects; - b3AlignedObjectArray m_cachedVisualShapes; - b3AlignedObjectArray m_cachedCollisionShapes; + btAlignedObjectArray m_cachedContactPoints; + btAlignedObjectArray m_cachedOverlappingObjects; + btAlignedObjectArray m_cachedVisualShapes; + btAlignedObjectArray m_cachedCollisionShapes; - b3AlignedObjectArray m_cachedVREvents; - b3AlignedObjectArray m_cachedKeyboardEvents; - b3AlignedObjectArray m_cachedMouseEvents; - b3AlignedObjectArray m_cachedMassMatrix; - b3AlignedObjectArray m_raycastHits; + btAlignedObjectArray m_cachedVREvents; + btAlignedObjectArray m_cachedKeyboardEvents; + btAlignedObjectArray m_cachedMouseEvents; + btAlignedObjectArray m_cachedMassMatrix; + btAlignedObjectArray m_raycastHits; - b3AlignedObjectArray m_bodyIdsRequestInfo; - b3AlignedObjectArray m_constraintIdsRequestInfo; + btAlignedObjectArray m_bodyIdsRequestInfo; + btAlignedObjectArray m_constraintIdsRequestInfo; + + btAlignedObjectArray m_userDataIdsRequestInfo; SharedMemoryStatus m_tempBackupServerStatus; @@ -1285,6 +1305,33 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { { break; } + case CMD_SYNC_USER_DATA_FAILED: + { + b3Warning("Synchronizing user data failed."); + break; + } + case CMD_REQUEST_USER_DATA_FAILED: + { + b3Warning("Requesting user data failed"); + break; + } + case CMD_ADD_USER_DATA_FAILED: + { + b3Warning("Adding user data failed (do the specified body and link exist?)"); + break; + } + case CMD_REMOVE_USER_DATA_FAILED: + { + b3Warning("Removing user data failed"); + break; + } + case CMD_REQUEST_USER_DATA_COMPLETED: + case CMD_SYNC_USER_DATA_COMPLETED: + case CMD_REMOVE_USER_DATA_COMPLETED: + case CMD_ADD_USER_DATA_COMPLETED: + { + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); @@ -1336,6 +1383,94 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { } } + if (serverCmd.m_type == CMD_SYNC_USER_DATA_COMPLETED) { + B3_PROFILE("CMD_SYNC_USER_DATA_COMPLETED"); + 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_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(); + + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + command.m_type = CMD_REQUEST_USER_DATA; + command.m_userDataRequestArgs = userDataGlobalId; + submitClientCommand(command); + return 0; + } + } + + 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]; + 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) { + // Only replace the value. + userDataPtr->replaceValue(dataStream, serverCmd.m_userDataResponseArgs.m_valueLength, serverCmd.m_userDataResponseArgs.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); + } + } + } + + if (serverCmd.m_type == CMD_REQUEST_USER_DATA_COMPLETED) { + + 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]; + 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; + submitClientCommand(command); + return 0; + } + m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus; + } + + 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); + } + } + } + } + if (serverCmd.m_type == CMD_REMOVE_BODY_COMPLETED) { for (int i=0;im_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]; + if (!userDataPtr) + { + return false; + } + 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 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]; + if (!userDataId) { + return -1; + } + return *userDataId; +} + +int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId, int linkIndex) 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(); +} + +void PhysicsClientSharedMemory::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const { + BodyJointInfoCache** 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()) + { + *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(); +} diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index ccc8c07ed..441fceb8d 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -83,6 +83,11 @@ 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; + }; #endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 969604f8c..6bf1004a0 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -13,11 +13,27 @@ #include "BodyJointInfoUtility.h" #include +#include "SharedMemoryUserData.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; + + ~BodyJointInfoCache2() { + } }; @@ -651,6 +667,38 @@ 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 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) { + // Only replace the value. + (userDataPtr)->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type); + } + 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); + } + } +} + void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd) { switch (serverCmd.m_type) @@ -1056,6 +1104,73 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd { break; } + case CMD_SYNC_USER_DATA_FAILED: + { + b3Warning("Synchronizing user data failed."); + break; + } + case CMD_ADD_USER_DATA_FAILED: + { + b3Warning("Adding user data failed (do the specified body and link exist?)"); + break; + } + case CMD_REMOVE_USER_DATA_FAILED: + { + b3Warning("Removing user data failed"); + break; + } + case CMD_ADD_USER_DATA_COMPLETED: + { + processAddUserData(serverCmd); + break; + } + case CMD_SYNC_USER_DATA_COMPLETED: + { + B3_PROFILE("CMD_SYNC_USER_DATA_COMPLETED"); + const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers; + b3UserDataGlobalIdentifier *identifiers = new b3UserDataGlobalIdentifier[numIdentifiers]; + memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(b3UserDataGlobalIdentifier)); + + for (int i=0; im_tmpInfoRequestCommand.m_type = CMD_REQUEST_USER_DATA; + m_data->m_tmpInfoRequestCommand.m_userDataRequestArgs = 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); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + processAddUserData(m_data->m_tmpInfoStatus); + } + } + delete[] identifiers; + break; + } + 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); + } + } + } + break; + } default: { //b3Warning("Unknown server status type"); @@ -1331,3 +1446,71 @@ 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]; + if (!userDataPtr) + { + return false; + } + 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]; + if (!userDataId) { + return -1; + } + return *userDataId; +} + +int PhysicsDirect::getNumUserData(int bodyUniqueId, int linkIndex) 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(); +} + +void PhysicsDirect::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) 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()) + { + *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(); +} diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index f2699a6f5..b02c10da8 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -25,7 +25,9 @@ protected: bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand); void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); - + + void processAddUserData(const struct SharedMemoryStatus& serverCmd); + void postProcessStatus(const struct SharedMemoryStatus& serverCmd); void resetData(); @@ -110,6 +112,11 @@ 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; }; #endif //PHYSICS_DIRECT_H diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 0606588e6..b7a2dd13c 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -225,3 +225,19 @@ 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); +} + +int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { + return m_data->m_physicsClient->getCachedUserDataId(bodyUniqueId, linkIndex, key); +} + +int PhysicsLoopBack::getNumUserData(int bodyUniqueId, int linkIndex) const { + return m_data->m_physicsClient->getNumUserData(bodyUniqueId, linkIndex); +} + +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); +} + diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index f4591ed99..afa42817b 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -86,6 +86,11 @@ 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; }; #endif //PHYSICS_LOOP_BACK_H diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index a3cb2bdf6..0e1501b8a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -176,6 +176,74 @@ 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; @@ -191,7 +259,7 @@ struct InternalBodyData btAlignedObjectArray m_rigidBodyJoints; btAlignedObjectArray m_rigidBodyJointNames; btAlignedObjectArray m_rigidBodyLinkNames; - + btHashMap m_linkUserDataMap; #ifdef B3_ENABLE_TINY_AUDIO b3HashMap m_audioSources; @@ -216,6 +284,10 @@ struct InternalBodyData m_rigidBodyJoints.clear(); m_rigidBodyJointNames.clear(); m_rigidBodyLinkNames.clear(); + for(int i=0; i 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; km_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); + if (!userData) { + return hasStatus; + } + btAssert(bufferSizeInBytes >= userData->m_bytes.size()) + serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId = clientCmd.m_userDataRequestArgs; + 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; + + strcpy(serverStatusOut.m_userDataResponseArgs.m_key, userData->m_key.c_str()); + if (userData->m_bytes.size()) + { + memcpy(bufferServerToClient, &userData->m_bytes[0], userData->m_bytes.size()); + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_ADD_USER_DATA"); + serverStatusOut.m_type = CMD_ADD_USER_DATA_FAILED; + + if (clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId< 0 || clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId >= m_data->m_bodyHandles.getNumHandles()) + { + 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); + } + + const int userDataId = (*userDataPtr)->add(clientCmd.m_addUserDataRequestArgs.m_key, + 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_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); + + // Keep bufferServerToClient as-is. + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRemoveUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + 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); + 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; + } + serverStatusOut.m_removeUserDataResponseArgs = clientCmd.m_removeUserDataRequestArgs; + serverStatusOut.m_type = CMD_REMOVE_USER_DATA_COMPLETED; + return hasStatus; +} + bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; @@ -5804,6 +6008,12 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S } int flags = 0; + + if (clientCmd.m_updateFlags & MULT_BODY_HAS_FLAGS) + { + flags = clientCmd.m_createMultiBodyArgs.m_flags; + } + bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); if (ok) @@ -9461,6 +9671,26 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = processCustomCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); break; } + case CMD_SYNC_USER_DATA: + { + hasStatus = processSyncUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_USER_DATA: + { + hasStatus = processRequestUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_ADD_USER_DATA: + { + hasStatus = processAddUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REMOVE_USER_DATA: + { + hasStatus = processRemoveUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } default: { BT_PROFILE("CMD_UNKNOWN"); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 24f9f2401..903f2e4e1 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -83,6 +83,10 @@ protected: bool processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processSaveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processAddUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRemoveUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); int extractCollisionShapes(const class btCollisionShape* colShape, const class btTransform& transform, struct b3CollisionShapeData* collisionShapeBuffer, int maxCollisionShapes); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index d35d425cc..dd2434124 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -36,6 +36,7 @@ #define MAX_SDF_FILENAME_LENGTH 1024 #define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH #define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM +#define MAX_USER_DATA_KEY_LENGTH MAX_URDF_FILENAME_LENGTH struct TmpFloat3 { @@ -926,6 +927,7 @@ enum eCreateMultiBodyEnum { MULTI_BODY_HAS_BASE=1, MULT_BODY_USE_MAXIMAL_COORDINATES=2, + MULT_BODY_HAS_FLAGS=4, }; struct b3CreateMultiBodyArgs { @@ -947,7 +949,7 @@ struct b3CreateMultiBodyArgs int m_linkParentIndices[MAX_CREATE_MULTI_BODY_LINKS]; int m_linkJointTypes[MAX_CREATE_MULTI_BODY_LINKS]; double m_linkJointAxis[3*MAX_CREATE_MULTI_BODY_LINKS]; - + int m_flags; #if 0 std::string m_name; std::string m_sourceFile; @@ -976,6 +978,32 @@ struct b3StateSerializationArguments int m_stateId; }; +struct SyncUserDataArgs +{ + // User data identifiers stored in m_bulletStreamDataServerToClientRefactor + // as as array of b3UserDataGlobalIdentifier objects + int m_numUserDataIdentifiers; +}; + +struct UserDataResponseArgs +{ + b3UserDataGlobalIdentifier m_userDataGlobalId; + int m_valueType; + int m_valueLength; + char m_key[MAX_USER_DATA_KEY_LENGTH]; + // Value data stored in m_bulletStreamDataServerToClientRefactor. +}; + +struct AddUserDataRequestArgs +{ + int m_bodyUniqueId; + int m_linkIndex; + int m_valueType; + int m_valueLength; + char m_key[MAX_USER_DATA_KEY_LENGTH]; + // Value data stored in m_bulletStreamDataServerToClientRefactor. +}; + struct SharedMemoryCommand { int m_type; @@ -1032,6 +1060,9 @@ struct SharedMemoryCommand struct b3CustomCommand m_customCommandArgs; struct b3StateSerializationArguments m_loadStateArguments; struct RequestCollisionShapeDataArgs m_requestCollisionShapeDataArguments; + struct b3UserDataGlobalIdentifier m_userDataRequestArgs; + struct AddUserDataRequestArgs m_addUserDataRequestArgs; + struct b3UserDataGlobalIdentifier m_removeUserDataRequestArgs; }; }; @@ -1054,11 +1085,6 @@ struct SendOverlappingObjectsArgs int m_numRemainingOverlappingObjects; }; - - - - - struct SharedMemoryStatus { int m_type; @@ -1109,6 +1135,9 @@ struct SharedMemoryStatus struct b3StateSerializationArguments m_saveStateResultArgs; struct b3LoadSoftBodyResultArgs m_loadSoftBodyResultArguments; struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs; + struct SyncUserDataArgs m_syncUserDataArgs; + struct UserDataResponseArgs m_userDataResponseArgs; + struct b3UserDataGlobalIdentifier m_removeUserDataResponseArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index bfabbfb0c..4648314c7 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -5,7 +5,8 @@ ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures ///my convention is year/month/day/rev -#define SHARED_MEMORY_MAGIC_NUMBER 201801170 +#define SHARED_MEMORY_MAGIC_NUMBER 201806020 +//#define SHARED_MEMORY_MAGIC_NUMBER 201801170 //#define SHARED_MEMORY_MAGIC_NUMBER 201801080 //#define SHARED_MEMORY_MAGIC_NUMBER 201801010 //#define SHARED_MEMORY_MAGIC_NUMBER 201710180 @@ -83,9 +84,14 @@ enum EnumSharedMemoryClientCommand CMD_SAVE_STATE, CMD_RESTORE_STATE, CMD_REQUEST_COLLISION_SHAPE_INFO, + + CMD_SYNC_USER_DATA, + CMD_REQUEST_USER_DATA, + CMD_ADD_USER_DATA, + CMD_REMOVE_USER_DATA, + //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, - }; enum EnumSharedMemoryServerStatus @@ -192,6 +198,15 @@ enum EnumSharedMemoryServerStatus CMD_COLLISION_SHAPE_INFO_FAILED, CMD_LOAD_SOFT_BODY_FAILED, CMD_LOAD_SOFT_BODY_COMPLETED, + + CMD_SYNC_USER_DATA_COMPLETED, + CMD_SYNC_USER_DATA_FAILED, + CMD_REQUEST_USER_DATA_COMPLETED, + CMD_REQUEST_USER_DATA_FAILED, + CMD_ADD_USER_DATA_COMPLETED, + CMD_ADD_USER_DATA_FAILED, + CMD_REMOVE_USER_DATA_COMPLETED, + CMD_REMOVE_USER_DATA_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; @@ -254,6 +269,27 @@ struct b3JointInfo }; +enum UserDataValueType { + // Data represents generic byte array. + USER_DATA_VALUE_TYPE_BYTES = 0, + // Data represents C-string + USER_DATA_VALUE_TYPE_STRING = 1, +}; + +struct b3UserDataValue +{ + int m_type; + int m_length; + 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 new file mode 100644 index 000000000..a29f59eb0 --- /dev/null +++ b/examples/SharedMemory/SharedMemoryUserData.h @@ -0,0 +1,48 @@ +#ifndef SHARED_MEMORY_USER_DATA_H +#define SHARED_MEMORY_USER_DATA_H + +#include +#include "LinearMath/btAlignedObjectArray.h" +#include "SharedMemoryPublic.h" + +struct SharedMemoryUserData +{ + std::string m_key; + int m_type; + + btAlignedObjectArray m_bytes; + + SharedMemoryUserData() + :m_type(-1) + { + } + + // Takes ownership of the passed key and value arguments. + SharedMemoryUserData(const char* key) + :m_key(key) + { + } + + // Takes ownership of the data pointed to by newValue. + void replaceValue(const char* bytes, int len, int type) + { + m_type = type; + m_bytes.resize(len); + for (int i=0;i0) + { + b3CreateMultiBodySetFlags(commandHandle, flags); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) @@ -8172,12 +8436,12 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, } else { + int szInBytes = sizeof(double) * szJointDamping; + int i; if (szJointDamping != dofCount) { printf("calculateInverseKinematics: the size of input joint damping values should be equal to the number of degrees of freedom, ignoring the additonal values."); } - int szInBytes = sizeof(double) * szJointDamping; - int i; jointDamping = (double*)malloc(szInBytes); for (i = 0; i < szJointDamping; i++) { @@ -8821,6 +9085,34 @@ static PyMethodDef SpamMethods[] = { "syncBodyInfo(physicsClientId=0)\n" "Update body and constraint/joint information, in case other clients made changes."}, + {"syncUserData", (PyCFunction)pybullet_syncUserData, METH_VARARGS | METH_KEYWORDS, + "syncUserData(physicsClientId=0)\n" + "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."}, + + {"getUserData", (PyCFunction)pybullet_getUserData, METH_VARARGS | METH_KEYWORDS, + "getUserData(bodyUniqueId, linkIndex, userDataId, physicsClientId=0)\n" + "Returns the user data value."}, + + {"removeUserData", (PyCFunction)pybullet_removeUserData, METH_VARARGS | METH_KEYWORDS, + "removeUserData(bodyUniqueId, linkIndex, 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."}, + + {"getNumUserData", (PyCFunction)pybullet_getNumUserData, METH_VARARGS | METH_KEYWORDS, + "getNumUserData(bodyUniqueId, linkIndex, physicsClientId=0)\n" + "Retrieves the number of user data entries in a link."}, + + {"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)."}, + {"removeBody", (PyCFunction)pybullet_removeBody, METH_VARARGS | METH_KEYWORDS, "Remove a body by its body unique id."}, diff --git a/src/LinearMath/btHashMap.h b/src/LinearMath/btHashMap.h index 5e9cdb605..f43e76a80 100644 --- a/src/LinearMath/btHashMap.h +++ b/src/LinearMath/btHashMap.h @@ -30,6 +30,11 @@ struct btHashString return m_hash; } + btHashString() + { + m_string=""; + m_hash=0; + } btHashString(const char* name) :m_string(name) {