add texture support in SDF, URDF for both OpenGL and software renderer (TinyRenderer)

data/sphere2.urdf

@@ -9,7 +9,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-      <sphere radius="0.5"/>
+<mesh filename="textured_sphere_smooth.obj" scale="0.5 0.5 0.5"/>      
       </geometry>
     </visual>
     <collision>

data/two_cubes.sdf

@@ -122,11 +122,13 @@
         </collision>
         <visual name='visual'>
           <geometry>
-            <box>
+          <mesh>
-              <size>1 1 1</size>
+            <scale>1 1 1</scale>
-            </box>
+            <uri>cube.obj</uri>
-          </geometry>
+          </mesh>
+        </geometry>
           <material>
+            <diffuse>1 1 1 1</diffuse>
             <script>
               <name>Gazebo/Grey</name>
               <uri>file://media/materials/scripts/gazebo.material</uri>
@@ -172,17 +174,15 @@
             </friction>
           </surface>
         </collision>
-        <visual name='visual'>
+         <visual name='visual'>
           <geometry>
-            <box>
+          <mesh>
-              <size>1 1 1</size>
+            <scale>1 1 1</scale>
-            </box>
+            <uri>cube.obj</uri>
-          </geometry>
+          </mesh>
+        </geometry>
           <material>
-            <script>
+            <diffuse>1 1 1 1</diffuse>
-              <name>Gazebo/Grey</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
           </material>
         </visual>
         <self_collide>0</self_collide>

examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp

@@ -1,11 +1,8 @@
 #include "b3ImportMeshUtility.h"
 
 #include <vector>
-#include "../../OpenGLWindow/GLInstancingRenderer.h"
 #include"Wavefront/tiny_obj_loader.h"
-#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "LinearMath/btVector3.h"
-#include "btBulletDynamicsCommon.h"
-#include "../../OpenGLWindow/SimpleOpenGL3App.h"
 #include "../ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h"
 #include "../../Utils/b3ResourcePath.h"
 #include "Bullet3Common/b3FileUtils.h"
@@ -88,28 +85,4 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
 	return false;
 }
 
-int b3ImportMeshUtility::loadAndRegisterMeshFromFile(const std::string& fileName, CommonRenderInterface* renderer)
-{
-	int shapeId = -1;
 
-	b3ImportMeshData meshData;
-	if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
-	{
-		int textureIndex = -1;
-
-		if (meshData.m_textureImage)
-		{
-			textureIndex = renderer->registerTexture(meshData.m_textureImage,meshData.m_textureWidth,meshData.m_textureHeight);
-		}
-
-		shapeId = renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], 
-									meshData.m_gfxShape->m_numvertices, 
-									&meshData.m_gfxShape->m_indices->at(0), 
-									meshData.m_gfxShape->m_numIndices,
-									B3_GL_TRIANGLES,
-									textureIndex);
-		delete meshData.m_gfxShape;
-		delete meshData.m_textureImage;
-	}
-	return shapeId;
-}

examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h

@@ -15,7 +15,6 @@ struct b3ImportMeshData
 class b3ImportMeshUtility
 {
 public:
-static int loadAndRegisterMeshFromFile(const std::string& fileName, class CommonRenderInterface* renderer);
 
 static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData);
 

examples/Importers/ImportObjDemo/ImportObjExample.cpp

@@ -56,7 +56,31 @@ ImportObjSetup::~ImportObjSetup()
 
 
 
-
+int loadAndRegisterMeshFromFile2(const std::string& fileName, CommonRenderInterface* renderer)
+{
+	int shapeId = -1;
+	
+	b3ImportMeshData meshData;
+	if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
+	{
+		int textureIndex = -1;
+		
+		if (meshData.m_textureImage)
+		{
+			textureIndex = renderer->registerTexture(meshData.m_textureImage,meshData.m_textureWidth,meshData.m_textureHeight);
+		}
+		
+		shapeId = renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], 
+										  meshData.m_gfxShape->m_numvertices, 
+										  &meshData.m_gfxShape->m_indices->at(0), 
+										  meshData.m_gfxShape->m_numIndices,
+										  B3_GL_TRIANGLES,
+										  textureIndex);
+		delete meshData.m_gfxShape;
+		delete meshData.m_textureImage;
+	}
+	return shapeId;
+}
 
 
 
@@ -77,7 +101,7 @@ void ImportObjSetup::initPhysics()
     btVector3 scaling(1,1,1);
 	btVector3 color(1,1,1);
 		
-   int shapeId = b3ImportMeshUtility::loadAndRegisterMeshFromFile(m_fileName, m_guiHelper->getRenderInterface());    
+   int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface());    
    if (shapeId>=0)
    {
         //int id = 

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -13,7 +13,7 @@ subject to the following restrictions:
 
 
 #include "BulletUrdfImporter.h"
-
+#include "../../CommonInterfaces/CommonRenderInterface.h"
 
 #include "URDFImporterInterface.h"
 #include "btBulletCollisionCommon.h"
@@ -26,12 +26,20 @@ subject to the following restrictions:
 #include <string>
 #include "../../Utils/b3ResourcePath.h"
 
+#include "../ImportMeshUtility/b3ImportMeshUtility.h"
 
 
 #include <iostream>
 #include <fstream>
 #include "UrdfParser.h"
 
+struct MyTexture
+{
+	int m_width;
+	int m_height;
+	unsigned char* textureData;
+};
+
 struct BulletURDFInternalData
 {
 	UrdfParser m_urdfParser;
@@ -589,7 +597,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 }
 
 
-static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
+static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture>& texturesOut)
 {
 
 	
@@ -693,7 +701,23 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
 						{
                             case FILE_OBJ:
                             {
-                                glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
+//                                glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
+						
+								b3ImportMeshData meshData;
+								if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
+								{
+									
+									if (meshData.m_textureImage)
+									{
+										MyTexture texData;
+										texData.m_width = meshData.m_textureWidth;
+										texData.m_height = meshData.m_textureHeight;
+										texData.textureData = meshData.m_textureImage;
+										texturesOut.push_back(texData);
+									}
+									glmesh = meshData.m_gfxShape;
+								}
+								
                                 break;
                             }
                            
@@ -914,7 +938,8 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
     btAlignedObjectArray<GLInstanceVertex> vertices;
 	btAlignedObjectArray<int> indices;
 	btTransform startTrans; startTrans.setIdentity();
-
+	btAlignedObjectArray<MyTexture> textures;
+	
     const UrdfModel& model = m_data->m_urdfParser.getModel();
 	UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
 	if (linkPtr)
@@ -934,14 +959,34 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
 				//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
 				m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
 			}
-			convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices);
+			convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
 		
 		
 		}
 	}
 	if (vertices.size() && indices.size())
 	{
-		graphicsIndex  = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
+//		graphicsIndex  = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
+		//graphicsIndex  = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
+		
+		CommonRenderInterface* renderer = m_data->m_guiHelper->getRenderInterface();
+		
+		if (renderer)
+		{
+			int textureIndex = -1;
+			if (textures.size())
+			{
+				textureIndex = renderer->registerTexture(textures[0].textureData,textures[0].m_width,textures[0].m_height);
+			}
+			graphicsIndex = renderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(),B3_GL_TRIANGLES,textureIndex);
+			
+		}
+	}
+	
+	//delete textures
+	for (int i=0;i<textures.size();i++)
+	{
+		delete textures[i].textureData;
 	}
 	return graphicsIndex;
 }

examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp

@@ -128,7 +128,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
 		
 		if (gFileNameArray.size()==0)
 		{
-			gFileNameArray.push_back("r2d2.urdf");
+			gFileNameArray.push_back("sphere2.urdf");
 
 		}
 

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -28,7 +28,7 @@ subject to the following restrictions:
 #include "../Utils/b3ResourcePath.h"
 #include "../TinyRenderer/TinyRenderer.h"
 #include "../OpenGLWindow/SimpleCamera.h"
-
+#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
 #include <iostream>
 #include <fstream>
 #include "../Importers/ImportURDFDemo/UrdfParser.h"
@@ -41,6 +41,13 @@ enum MyFileType
     MY_FILE_OBJ=3,
 };
 
+struct MyTexture2
+{
+	unsigned char* textureData;
+	int m_width;
+	int m_height;
+};
+
 struct TinyRendererObjectArray
 {
   btAlignedObjectArray<  TinyRenderObjectData*> m_renderObjects;
@@ -95,7 +102,7 @@ TinyRendererVisualShapeConverter::~TinyRendererVisualShapeConverter()
 
 
 
-void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
+void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut)
 {
 
 	
@@ -201,7 +208,23 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 						{
                             case MY_FILE_OBJ:
                             {
-                                glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
+                                //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
+								b3ImportMeshData meshData;
+								if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
+								{
+									
+									if (meshData.m_textureImage)
+									{
+										MyTexture2 texData;
+										texData.m_width = meshData.m_textureWidth;
+										texData.m_height = meshData.m_textureHeight;
+										texData.textureData = meshData.m_textureImage;
+										texturesOut.push_back(texData);
+									}
+									glmesh = meshData.m_gfxShape;
+								}
+
+								
                                 break;
                             }
                            
@@ -420,11 +443,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj)
 {
     
-	btAlignedObjectArray<GLInstanceVertex> vertices;
+	
-	btAlignedObjectArray<int> indices;
-	btTransform startTrans; startTrans.setIdentity();
-	int graphicsIndex = -1;
-
 	UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
 	if (linkPtr)
 	{
@@ -433,6 +452,12 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
 	
 		for (int v = 0; v < link->m_visualArray.size();v++)
 		{
+			btAlignedObjectArray<MyTexture2> textures;
+			btAlignedObjectArray<GLInstanceVertex> vertices;
+			btAlignedObjectArray<int> indices;
+			btTransform startTrans; startTrans.setIdentity();
+			int graphicsIndex = -1;
+
 			const UrdfVisual& vis = link->m_visualArray[v];
 			btTransform childTrans = vis.m_linkLocalFrame;
 			btHashString matName(vis.m_materialName.c_str());
@@ -458,14 +483,29 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
             btAssert(visualsPtr);
             TinyRendererObjectArray* visuals = *visualsPtr;
             
-			convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices);
+			convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
 
             if (vertices.size() && indices.size())
             {
                 TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer);
-                tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor);
+				unsigned char* textureImage=0;
+				int textureWidth=0;
+				int textureHeight=0;
+				if (textures.size())
+				{
+					textureImage = textures[0].textureData;
+					textureWidth = textures[0].m_width;
+					textureHeight = textures[0].m_height;
+				}
+				
+                tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor,
+										   textureImage,textureWidth,textureHeight);
                 visuals->m_renderObjects.push_back(tinyObj);
             }
+			for (int i=0;i<textures.size();i++)
+			{
+				delete textures[i].textureData;
+			}
 		}
 	}
 }