Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-08-08 22:39:29 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #4178 from RanTig/rb-userdata

Browse Source 
Also adds user data specified in URDF files to rigid bodies.
This commit is contained in:
erwincoumans
2022-02-24 11:21:46 -08:00
committed by GitHub
parent 58cde7b598 d26dbef2dc
commit 267f983498
1 changed files with 32 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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)
{
const UrdfLink* link = *urdfModel->m_links.getAtIndex(i);
int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str());
if (linkIndex)
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)
{
addUserData(link->m_userData, bodyUniqueId, *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)
{
for (int i = 0; i < urdfModel->m_links.size(); ++i)
{
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);
}
}
}