diff options
author | erwincoumans <erwin.coumans@gmail.com> | 2022-02-24 11:21:46 -0800 |
---|---|---|
committer | GitHub <noreply@github.com> | 2022-02-24 11:21:46 -0800 |
commit | 267f983498c5a249838cd614a01c627b3adda293 (patch) | |
tree | d17a7286af0becd852862988b11839576a1dc6fd | |
parent | 58cde7b598732bbb0d6c913d0d68ee4fb39afc4e (diff) | |
parent | d26dbef2dc5a88fd2642157a165ed7735470aa5f (diff) | |
download | bullet3-267f983498c5a249838cd614a01c627b3adda293.tar.gz |
Merge pull request #4178 from RanTig/rb-userdata
Also adds user data specified in URDF files to rigid bodies.
-rw-r--r-- | examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 52 |
1 files changed, 32 insertions, 20 deletions
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b33a29d9b..aade5904c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3433,34 +3433,46 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, } } - // Because the link order between UrdfModel and MultiBody may be different, - // create a mapping from link name to link index in order to apply the user - // data to the correct link in the MultiBody. - btHashMap<btHashString, int> linkNameToIndexMap; - if (bodyHandle->m_multiBody) - { - btMultiBody* mb = bodyHandle->m_multiBody; - linkNameToIndexMap.insert(mb->getBaseName(), -1); - for (int linkIndex = 0; linkIndex < mb->getNumLinks(); ++linkIndex) - { - linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex); - } - } - + // Add user data specified in URDF to the added body. const UrdfModel* urdfModel = u2b.getUrdfModel(); if (urdfModel) { addUserData(urdfModel->m_userData, bodyUniqueId); - for (int i = 0; i < urdfModel->m_links.size(); ++i) + if (bodyHandle->m_multiBody) + { + btMultiBody* mb = bodyHandle->m_multiBody; + // Because the link order between UrdfModel and MultiBody may be different, + // create a mapping from link name to link index in order to apply the user + // data to the correct link in the MultiBody. + btHashMap<btHashString, int> linkNameToIndexMap; + linkNameToIndexMap.insert(mb->getBaseName(), -1); + for (int linkIndex = 0; linkIndex < mb->getNumLinks(); ++linkIndex) + { + linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex); + } + for (int i = 0; i < urdfModel->m_links.size(); ++i) + { + const UrdfLink* link = *urdfModel->m_links.getAtIndex(i); + int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str()); + if (linkIndex) + { + addUserData(link->m_userData, bodyUniqueId, *linkIndex); + for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex) + { + addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, *linkIndex, visualShapeIndex); + } + } + } + } + else if (bodyHandle->m_rigidBody) { - const UrdfLink* link = *urdfModel->m_links.getAtIndex(i); - int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str()); - if (linkIndex) + for (int i = 0; i < urdfModel->m_links.size(); ++i) { - addUserData(link->m_userData, bodyUniqueId, *linkIndex); + const UrdfLink* link = *urdfModel->m_links.getAtIndex(i); + addUserData(link->m_userData, bodyUniqueId, -1); for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex) { - addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, *linkIndex, visualShapeIndex); + addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, -1, visualShapeIndex); } } } |