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

Also adds user data specified in URDF files to rigid bodies (previously,

Browse Source 
only multi bodies were added correctly).
This commit is contained in:
Tigran Gasparian
2022-02-23 16:45:52 +01:00
parent 88651395b7
commit d26dbef2dc
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);
}
}
}