diff options
author | Tigran Gasparian <gaspariantigran@gmail.com> | 2022-02-23 16:45:52 +0100 |
---|---|---|
committer | Tigran Gasparian <gaspariantigran@gmail.com> | 2022-02-23 16:45:52 +0100 |
commit | d26dbef2dc5a88fd2642157a165ed7735470aa5f (patch) | |
tree | a411c00aad1006476f8d5679b8c27fb6331df08d | |
parent | 88651395b7afa2c5013d6d1a6430b35f309496e8 (diff) | |
download | bullet3-d26dbef2dc5a88fd2642157a165ed7735470aa5f.tar.gz |
Also adds user data specified in URDF files to rigid bodies (previously,
only multi bodies were added correctly).
-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 357101303..ae3142e15 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); } } } |