Merge pull request #4178 from RanTig/rb-userdata

Also adds user data specified in URDF files to rigid bodies.

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);
 					}
 				}
 			}