Merge pull request #1424 from erwincoumans/master

expose COV_ENABLE_Y_AXIS_UP, obj2sdf, add option to merge shapes (better performance), tiny_obj_loader support transparency ("d" and "Tr")

Extras/obj2sdf/obj2sdf.cpp

@@ -13,6 +13,22 @@
 #include "Bullet3Common/b3FileUtils.h"
 #include "../Utils/b3ResourcePath.h"
 #include "Bullet3Common/b3CommandLineArgs.h"
+#include "Bullet3Common/b3HashMap.h"
+
+struct ShapeContainer
+{
+	std::string m_matName;
+	std::string m_shapeName;
+	tinyobj::material_t material;
+	std::vector<float>          positions;
+	std::vector<float>          normals;
+	std::vector<float>          texcoords;
+	std::vector<unsigned int>   indices;
+
+	b3AlignedObjectArray<int> m_shapeIndices;
+};
+
+b3HashMap<b3HashString, ShapeContainer> gMaterialNames;
 
 #define MAX_PATH_LEN 1024
 
@@ -53,6 +69,8 @@ int main(int argc, char* argv[])
 		printf("Please use --fileName=\"pathToObj\".");
 		exit(0);
 	}
+	bool mergeMaterials = args.CheckCmdLineFlag("mergeMaterials");
+	
 	char fileNameWithPath[MAX_PATH_LEN];
 	bool fileFound = (b3ResourcePath::findResourcePath(fileName,fileNameWithPath,MAX_PATH_LEN))>0;
 	char materialPrefixPath[MAX_PATH_LEN];
@@ -72,6 +90,220 @@ int main(int argc, char* argv[])
 
 	fprintf(sdfFile, "<sdf version='1.6'>\n\t<world name='default'>\n\t<gravity>0 0 -9.8</gravity>\n");
 
+	for (int s = 0; s < (int)shapes.size(); s++)
+	{
+		tinyobj::shape_t& shape = shapes[s];
+		tinyobj::material_t mat = shape.material;
+		
+		b3HashString key = mat.name.length() ? mat.name.c_str() : "";
+		if (!gMaterialNames.find(key))
+		{
+			ShapeContainer container;
+			container.m_matName = mat.name;
+			container.m_shapeName = shape.name;
+			container.material = mat;
+			gMaterialNames.insert(key, container);
+		}
+
+		ShapeContainer* shapeC = gMaterialNames.find(key);
+		if (shapeC)
+		{
+			shapeC->m_shapeIndices.push_back(s);
+		
+			int curPositions = shapeC->positions.size()/3;
+			int curNormals = shapeC->normals.size()/3;
+			int curTexcoords = shapeC->texcoords.size()/2;
+
+			int faceCount = shape.mesh.indices.size();
+			int vertexCount = shape.mesh.positions.size();
+			for (int v = 0; v < vertexCount; v++)
+			{
+				shapeC->positions.push_back(shape.mesh.positions[v]);
+			}
+			int numNormals = int(shape.mesh.normals.size());
+			for (int vn = 0; vn < numNormals; vn++)
+			{
+				shapeC->normals.push_back(shape.mesh.normals[vn]);
+			}
+			int numTexCoords = int(shape.mesh.texcoords.size());
+			for (int vt = 0; vt < numTexCoords; vt++)
+			{
+				shapeC->texcoords.push_back(shape.mesh.texcoords[vt]);
+			}
+
+			for (int face = 0; face < faceCount; face += 3)
+			{
+				if (face < 0 && face >= int(shape.mesh.indices.size()))
+				{
+					continue;
+				}
+
+				shapeC->indices.push_back(shape.mesh.indices[face] + curPositions);
+				shapeC->indices.push_back(shape.mesh.indices[face+1] + curPositions);
+				shapeC->indices.push_back(shape.mesh.indices[face + 2] + curPositions);
+			}
+		}
+	}
+	
+	printf("unique materials=%d\n", gMaterialNames.size());
+
+	
+	if (mergeMaterials)
+	{
+		for (int m = 0; m < gMaterialNames.size();m++)
+		{
+			if (gMaterialNames.getAtIndex(m)->m_shapeIndices.size() == 0)
+				continue;
+
+			ShapeContainer* shapeCon =gMaterialNames.getAtIndex(m);
+			
+			printf("object name = %s\n", shapeCon->m_shapeName.c_str());
+			
+			char objSdfPartFileName[MAX_PATH_LEN];
+			sprintf(objSdfPartFileName, "part%d.obj", m);
+
+			char objFileName[MAX_PATH_LEN];
+			if (strlen(materialPrefixPath) > 0)
+			{
+				sprintf(objFileName, "%s/part%d.obj", materialPrefixPath, m);
+			}
+			else
+			{
+				sprintf(objFileName, "part%d.obj", m);
+			}
+
+
+			FILE* f = fopen(objFileName, "w");
+			if (f == 0)
+			{
+				printf("Fatal error: cannot create part obj file %s\n", objFileName);
+				exit(0);
+			}
+			fprintf(f, "# Exported using automatic converter by Erwin Coumans\n");
+			if (matLibName.length())
+			{
+				fprintf(f, "mtllib %s.mtl\n", matLibName.c_str());
+			}
+			else
+			{
+				fprintf(f, "mtllib bedroom.mtl\n");
+
+			}
+
+			int faceCount = shapeCon->indices.size();
+			int vertexCount = shapeCon->positions.size();
+			tinyobj::material_t mat = shapeCon->material;
+			if (shapeCon->m_matName.length())
+			{
+				const char* objName = shapeCon->m_matName.c_str();
+				printf("mat.name = %s\n", objName);
+				fprintf(f, "#object %s\n\n", objName);
+			}
+			for (int v = 0; v < vertexCount / 3; v++)
+			{
+				fprintf(f, "v %f %f %f\n", shapeCon->positions[v * 3 + 0], shapeCon->positions[v * 3 + 1], shapeCon->positions[v * 3 + 2]);
+			}
+
+			if (mat.name.length())
+			{
+				fprintf(f, "usemtl %s\n", mat.name.c_str());
+			}
+			else
+			{
+				fprintf(f, "usemtl wire_028089177\n");
+			}
+
+			fprintf(f, "\n");
+			int numNormals = int(shapeCon->normals.size());
+
+			for (int vn = 0; vn < numNormals / 3; vn++)
+			{
+				fprintf(f, "vn %f %f %f\n", shapeCon->normals[vn * 3 + 0], shapeCon->normals[vn * 3 + 1], shapeCon->normals[vn * 3 + 2]);
+			}
+
+			fprintf(f, "\n");
+			int numTexCoords = int(shapeCon->texcoords.size());
+			for (int vt = 0; vt < numTexCoords / 2; vt++)
+			{
+				fprintf(f, "vt %f %f\n", shapeCon->texcoords[vt * 2 + 0], shapeCon->texcoords[vt * 2 + 1]);
+			}
+
+			fprintf(f, "s off\n");
+
+			for (int face = 0; face < faceCount; face += 3)
+			{
+				if (face < 0 && face >= int(shapeCon->indices.size()))
+				{
+					continue;
+				}
+				fprintf(f, "f %d/%d/%d %d/%d/%d %d/%d/%d\n",
+					shapeCon->indices[face] + 1, shapeCon->indices[face] + 1, shapeCon->indices[face] + 1,
+					shapeCon->indices[face + 1] + 1, shapeCon->indices[face + 1] + 1, shapeCon->indices[face + 1] + 1,
+					shapeCon->indices[face + 2] + 1, shapeCon->indices[face + 2] + 1, shapeCon->indices[face + 2] + 1);
+			}
+			fclose(f);
+
+			float kdRed = mat.diffuse[0];
+			float kdGreen = mat.diffuse[1];
+			float kdBlue = mat.diffuse[2];
+			float transparency = mat.transparency;
+			
+			fprintf(sdfFile, "\t\t<model name='%s'>\n"
+				"\t\t\t<static>1</static>\n"
+				"\t\t\t<pose frame=''>0 0 0 0 0 0</pose>\n"
+				"\t\t\t<link name='link_d%d'>\n"
+				"\t\t\t<inertial>\n"
+				"\t\t\t<mass>0</mass>\n"
+				"\t\t\t<inertia>\n"
+				"\t\t\t<ixx>0.166667</ixx>\n"
+				"\t\t\t<ixy>0</ixy>\n"
+				"\t\t\t<ixz>0</ixz>\n"
+				"\t\t\t<iyy>0.166667</iyy>\n"
+				"\t\t\t<iyz>0</iyz>\n"
+				"\t\t\t<izz>0.166667</izz>\n"
+				"\t\t\t</inertia>\n"
+				"\t\t\t</inertial>\n"
+				"\t\t\t<collision concave='yes' name='collision_%d'>\n"
+				"\t\t\t<geometry>\n"
+				"\t\t\t<mesh>\n"
+				"\t\t\t<scale>1 1 1</scale>\n"
+				"\t\t\t\t<uri>%s</uri>\n"
+				"\t\t\t</mesh>\n"
+				"\t\t\t</geometry>\n"
+				"\t\t\t  </collision>\n"
+				"\t\t\t<visual name='visual'>\n"
+				"\t\t\t\t<geometry>\n"
+				"\t\t\t\t<mesh>\n"
+				"\t\t\t\t\t<scale>1 1 1</scale>\n"
+				"\t\t\t\t\t<uri>%s</uri>\n"
+				"\t\t\t\t</mesh>\n"
+				"\t\t\t\t</geometry>\n"
+				"\t\t\t<material>\n"
+				"\t\t\t\t<ambient>1 0 0 1</ambient>\n"
+				"\t\t\t\t<diffuse>%f %f %f %f</diffuse>\n"
+				"\t\t\t\t<specular>0.1 0.1 0.1 1</specular>\n"
+				"\t\t\t\t<emissive>0 0 0 0</emissive>\n"
+				"\t\t\t </material>\n"
+				"\t\t\t </visual>\n"
+				"\t\t\t </link>\n"
+				"\t\t\t</model>\n", objSdfPartFileName, m, m,
+				objSdfPartFileName, objSdfPartFileName,
+				kdRed, kdGreen, kdBlue, transparency);
+
+
+		}
+
+
+
+
+
+
+
+
+	}
+	else
+	{
+
 		for (int s = 0; s < (int)shapes.size(); s++)
 		{
 			tinyobj::shape_t& shape = shapes[s];
@@ -85,7 +317,8 @@ int main(int argc, char* argv[])
 			if (strlen(materialPrefixPath) > 0)
 			{
 				sprintf(objFileName, "%s/part%d.obj", materialPrefixPath, s);
-		} else
+			}
+			else
 			{
 				sprintf(objFileName, "part%d.obj", s);
 			}
@@ -99,7 +332,8 @@ int main(int argc, char* argv[])
 			if (matLibName.length())
 			{
 				fprintf(f, "mtllib %s.mtl\n", matLibName.c_str());
-		} else
+			}
+			else
 			{
 				fprintf(f, "mtllib bedroom.mtl\n");
 
@@ -123,7 +357,8 @@ int main(int argc, char* argv[])
 			if (mat.name.length())
 			{
 				fprintf(f, "usemtl %s\n", mat.name.c_str());
-		} else
+			}
+			else
 			{
 				fprintf(f, "usemtl wire_028089177\n");
 			}
@@ -161,7 +396,7 @@ int main(int argc, char* argv[])
 			float kdRed = mat.diffuse[0];
 			float kdGreen = mat.diffuse[1];
 			float kdBlue = mat.diffuse[2];
-
+			float transparency = mat.transparency;
 			char objSdfPartFileName[MAX_PATH_LEN];
 			sprintf(objSdfPartFileName, "part%d.obj", s);
 			fprintf(sdfFile, "\t\t<model name='%s'>\n"
@@ -196,7 +431,7 @@ int main(int argc, char* argv[])
 				"\t\t\t\t</geometry>\n"
 				"\t\t\t<material>\n"
 				"\t\t\t\t<ambient>1 0 0 1</ambient>\n"
-          "\t\t\t\t<diffuse>%f %f %f 1</diffuse>\n"
+				"\t\t\t\t<diffuse>%f %f %f %f</diffuse>\n"
 				"\t\t\t\t<specular>0.1 0.1 0.1 1</specular>\n"
 				"\t\t\t\t<emissive>0 0 0 0</emissive>\n"
 				"\t\t\t </material>\n"
@@ -204,10 +439,11 @@ int main(int argc, char* argv[])
 				"\t\t\t </link>\n"
 				"\t\t\t</model>\n", objSdfPartFileName, s, s,
 				objSdfPartFileName, objSdfPartFileName,
-			kdRed, kdGreen, kdBlue);
+				kdRed, kdGreen, kdBlue, transparency);
 
 
 		}
+	}
 	fprintf(sdfFile,"\t</world>\n</sdf>\n");
 
 	fclose(sdfFile);

examples/RobotSimulator/MinitaurSetup.cpp

@@ -128,7 +128,7 @@ int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3V
 	{
 		b3JointInfo jointInfo;
 		sim->getJointInfo(m_data->m_quadrupedUniqueId,i,&jointInfo);
-		if (jointInfo.m_jointName)
+		if (jointInfo.m_jointName[0])
 		{
 			m_data->m_jointNameToId.insert(jointInfo.m_jointName,i);
 		}

examples/SharedMemory/SharedMemoryCommands.h

@@ -32,7 +32,6 @@
 #define MAX_SDF_FILENAME_LENGTH 1024
 #define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
 #define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
-#define MAX_SDF_BODIES 512
 
 struct TmpFloat3 
 {

examples/SharedMemory/SharedMemoryPublic.h

@@ -355,6 +355,8 @@ enum b3VREventType
 #define MAX_KEYBOARD_EVENTS 256
 #define MAX_MOUSE_EVENTS 256
 
+#define MAX_SDF_BODIES 1024
+
 
 enum b3VRButtonInfo
 {

examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp

@@ -328,6 +328,7 @@ void InitMaterial(material_t& material) {
     material.emission[i] = 0.f;
   }
   material.shininess = 1.f;
+  material.transparency = 1.f;
 }
 
 std::string LoadMtl (
@@ -465,6 +466,20 @@ std::string LoadMtl (
       continue;
     }
 
+	// transparency
+	if (token[0] == 'T' && token[1] == 'r' && isSpace(token[2])) {
+		token += 2;
+		material.transparency = parseFloat(token);
+		continue;
+	}
+
+	// transparency
+	if (token[0] == 'd' && isSpace(token[1])) {
+		token += 1;
+		material.transparency = parseFloat(token);
+		continue;
+	}
+
     // ambient texture
     if ((0 == strncmp(token, "map_Ka", 6)) && isSpace(token[6])) {
       token += 7;

examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h

@@ -22,6 +22,7 @@ typedef struct
     float transmittance[3];
     float emission[3];
     float shininess;
+	float transparency;
 
     std::string ambient_texname;
     std::string diffuse_texname;

examples/pybullet/pybullet.c

@@ -1,5 +1,4 @@
 
-
 #include "../SharedMemory/PhysicsClientC_API.h"
 #include "../SharedMemory/PhysicsDirectC_API.h"
 #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
@@ -571,7 +570,6 @@ static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args, PyObject* ke
 	return NULL;
 }
 
-#define MAX_SDF_BODIES 512
 static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	const char* bulletFileName = "";
@@ -701,7 +699,9 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key
 		b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
 	if (numBodies > MAX_SDF_BODIES)
 	{
-		PyErr_SetString(SpamError, "SDF exceeds body capacity");
+		char str[1024];
+		sprintf(str,"SDF exceeds body capacity: %d > %d", numBodies, MAX_SDF_BODIES);
+		PyErr_SetString(SpamError, str);
 		return NULL;
 	}
 
@@ -1197,7 +1197,9 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw
 		b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
 	if (numBodies > MAX_SDF_BODIES)
 	{
-		PyErr_SetString(SpamError, "SDF exceeds body capacity");
+		char str[1024];
+                sprintf(str,"SDF exceeds body capacity: %d > %d", numBodies, MAX_SDF_BODIES);
+		PyErr_SetString(SpamError, str);
 		return NULL;
 	}
 
@@ -8191,6 +8193,7 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "COV_ENABLE_VR_TELEPORTING", COV_ENABLE_VR_TELEPORTING);
 	PyModule_AddIntConstant(m, "COV_ENABLE_RENDERING", COV_ENABLE_RENDERING);
 	PyModule_AddIntConstant(m, "COV_ENABLE_TINY_RENDERER", COV_ENABLE_TINY_RENDERER);
+	PyModule_AddIntConstant(m, "COV_ENABLE_Y_AXIS_UP", COV_ENABLE_Y_AXIS_UP);
 
 	PyModule_AddIntConstant(m, "COV_ENABLE_VR_RENDER_CONTROLLERS", COV_ENABLE_VR_RENDER_CONTROLLERS);
 	PyModule_AddIntConstant(m, "COV_ENABLE_KEYBOARD_SHORTCUTS", COV_ENABLE_KEYBOARD_SHORTCUTS);

src/BulletCollision/CollisionShapes/btConvexShape.cpp

@@ -121,6 +121,10 @@ static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector
 	btScalar maxDot;
 	long ptIndex = vec.maxDot( points, numPoints, maxDot);
 	btAssert(ptIndex >= 0);
+	if (ptIndex<0)
+	{
+		ptIndex = 0;
+	}
 	btVector3 supVec = points[ptIndex] * localScaling;
 	return supVec;
 #endif //__SPU__

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -982,8 +982,14 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 		{
 			case btMultibodyLink::ePrismatic:
 			case btMultibodyLink::eRevolute:
+			{
+				if (D[0]>=SIMD_EPSILON)
 				{
 					invDi[0] = 1.0f / D[0];
+				} else
+				{
+					invDi[0] = 0;
+				}
 				break;
 			}
 			case btMultibodyLink::eSpherical:
@@ -1266,12 +1272,29 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo
     if (num_links == 0) 
 	{
 		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
+   
+	if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
+	{
 		result[0] = rhs_bot[0] / m_baseInertia[0];
 		result[1] = rhs_bot[1] / m_baseInertia[1];
         	result[2] = rhs_bot[2] / m_baseInertia[2];
+	} else
+	{
+		result[0] = 0;
+		result[1] = 0;
+		result[2] = 0;
+	}
+	if (m_baseMass>=SIMD_EPSILON)
+	{
         	result[3] = rhs_top[0] / m_baseMass;
         	result[4] = rhs_top[1] / m_baseMass;
         	result[5] = rhs_top[2] / m_baseMass;
+	} else
+	{
+		result[3] = 0;
+		result[4] = 0;
+		result[5] = 0;
+	}
     } else 
 	{
 		if (!m_cachedInertiaValid)
@@ -1322,9 +1345,21 @@ void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionV
     if (num_links == 0) 
 	{
 		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
+		if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
+        	{
 			result.setAngular(rhs.getAngular() / m_baseInertia);       
+        	} else  
+        	{       
+                	result.setAngular(btVector3(0,0,0));
+        	}
+        	if (m_baseMass>=SIMD_EPSILON)
+        	{       
 			result.setLinear(rhs.getLinear() / m_baseMass);               	 	
         	} else  
+        	{       
+                	result.setLinear(btVector3(0,0,0));
+        	}
+    	} else 
 	{
 		/// Special routine for calculating the inverse of a spatial inertia matrix
 		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices