diff --git a/data/threecubes/newsdf.sdf b/data/threecubes/newsdf.sdf new file mode 100644 index 0000000000..b30db2f162 --- /dev/null +++ b/data/threecubes/newsdf.sdf @@ -0,0 +1,122 @@ + + + 0 0 -9.8 + + 1 + 0 0 0 0 0 0 + + + 0 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 1 1 + part0.obj + + + + + + + 1 1 1 + part0.obj + + + + 1 0 0 1 + 0.000000 0.640000 0.000000 1.000000 + 0.1 0.1 0.1 1 + 0 0 0 0 + + + + + + 1 + 0 0 0 0 0 0 + + + 0 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 1 1 + part1.obj + + + + + + + 1 1 1 + part1.obj + + + + 1 0 0 1 + 0.000000 0.000000 0.640000 1.000000 + 0.1 0.1 0.1 1 + 0 0 0 0 + + + + + + 1 + 0 0 0 0 0 0 + + + 0 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + + + + + 1 1 1 + part2.obj + + + + + + + 1 1 1 + part2.obj + + + + 1 0 0 1 + 0.640000 0.000000 0.000000 1.000000 + 0.1 0.1 0.1 1 + 0 0 0 0 + + + + + + diff --git a/data/threecubes/part0.obj b/data/threecubes/part0.obj new file mode 100644 index 0000000000..db298babfe --- /dev/null +++ b/data/threecubes/part0.obj @@ -0,0 +1,60 @@ +# Exported using automatic converter by Erwin Coumans +mtllib three_cubes.mtl +#object Cube.001 + +v -4.474365 4.513344 -0.488734 +v -4.474365 2.513344 -0.488735 +v -4.474365 2.513344 1.511265 +v -2.474365 4.513344 -0.488734 +v -2.474365 2.513344 -0.488735 +v -4.474365 2.513344 -0.488735 +v -2.474365 4.513343 1.511266 +v -2.474365 2.513344 1.511265 +v -2.474365 2.513344 -0.488735 +v -4.474365 4.513343 1.511266 +v -4.474365 2.513344 1.511265 +v -2.474365 2.513344 1.511265 +v -4.474365 2.513344 -0.488735 +v -2.474365 4.513343 1.511266 +v -2.474365 4.513344 -0.488734 +v -4.474365 4.513343 1.511266 +v -4.474365 4.513344 -0.488734 +v -2.474365 4.513344 -0.488734 +v -2.474365 4.513343 1.511266 +v -4.474365 2.513344 1.511265 +usemtl Material.001 + + +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 0.000000 0.000000 +vt 0.000000 0.000000 +vt 0.000000 0.000000 +vt 0.000000 0.000000 +vt 0.000000 0.000000 +s off +f 1/1/1 2/2/2 3/3/3 +f 4/4/4 5/5/5 6/6/6 +f 7/7/7 8/8/8 9/9/9 +f 10/10/10 11/11/11 12/12/12 +f 13/13/13 5/5/5 12/12/12 +f 10/10/10 14/14/14 15/15/15 +f 16/16/16 1/1/1 3/3/3 +f 17/17/17 4/4/4 6/6/6 +f 18/18/18 7/7/7 9/9/9 +f 19/19/19 10/10/10 12/12/12 +f 20/20/20 13/13/13 12/12/12 +f 17/17/17 10/10/10 15/15/15 diff --git a/data/threecubes/part1.obj b/data/threecubes/part1.obj new file mode 100644 index 0000000000..fb43c3a76f --- /dev/null +++ b/data/threecubes/part1.obj @@ -0,0 +1,60 @@ +# Exported using automatic converter by Erwin Coumans +mtllib three_cubes.mtl +#object Cube.002 + +v -4.474365 4.513344 -3.663786 +v -4.474365 2.513345 -3.663786 +v -4.474365 2.513344 -1.663787 +v -2.474365 4.513344 -3.663786 +v -2.474365 2.513345 -3.663786 +v -4.474365 2.513345 -3.663786 +v -2.474365 4.513344 -1.663786 +v -2.474365 2.513344 -1.663787 +v -2.474365 2.513345 -3.663786 +v -4.474365 4.513344 -1.663786 +v -4.474365 2.513344 -1.663787 +v -2.474365 2.513344 -1.663787 +v -4.474365 2.513345 -3.663786 +v -2.474365 4.513344 -1.663786 +v -2.474365 4.513344 -3.663786 +v -4.474365 4.513344 -1.663786 +v -4.474365 4.513344 -3.663786 +v -2.474365 4.513344 -3.663786 +v -2.474365 4.513344 -1.663786 +v -4.474365 2.513344 -1.663787 +usemtl Material.002 + + +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 0.000000 0.000000 +vt 0.000000 0.000000 +vt 0.000000 0.000000 +vt 0.000000 0.000000 +vt 0.000000 0.000000 +s off +f 1/1/1 2/2/2 3/3/3 +f 4/4/4 5/5/5 6/6/6 +f 7/7/7 8/8/8 9/9/9 +f 10/10/10 11/11/11 12/12/12 +f 13/13/13 5/5/5 12/12/12 +f 10/10/10 14/14/14 15/15/15 +f 16/16/16 1/1/1 3/3/3 +f 17/17/17 4/4/4 6/6/6 +f 18/18/18 7/7/7 9/9/9 +f 19/19/19 10/10/10 12/12/12 +f 20/20/20 13/13/13 12/12/12 +f 17/17/17 10/10/10 15/15/15 diff --git a/data/threecubes/part2.obj b/data/threecubes/part2.obj new file mode 100644 index 0000000000..a5020d50d9 --- /dev/null +++ b/data/threecubes/part2.obj @@ -0,0 +1,60 @@ +# Exported using automatic converter by Erwin Coumans +mtllib three_cubes.mtl +#object Cube + +v -4.474365 4.513343 2.535691 +v -4.474365 2.513344 2.535691 +v -4.474365 2.513343 4.535690 +v -2.474365 4.513343 2.535691 +v -2.474365 2.513344 2.535691 +v -4.474365 2.513344 2.535691 +v -2.474365 4.513343 4.535691 +v -2.474365 2.513343 4.535690 +v -2.474365 2.513344 2.535691 +v -4.474365 4.513343 4.535691 +v -4.474365 2.513343 4.535690 +v -2.474365 2.513343 4.535690 +v -4.474365 2.513344 2.535691 +v -2.474365 4.513343 4.535691 +v -2.474365 4.513343 2.535691 +v -4.474365 4.513343 4.535691 +v -4.474365 4.513343 2.535691 +v -2.474365 4.513343 2.535691 +v -2.474365 4.513343 4.535691 +v -4.474365 2.513343 4.535690 +usemtl Material + + +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 0.000000 0.000000 +vt 0.000000 0.000000 +vt 0.000000 0.000000 +vt 0.000000 0.000000 +vt 0.000000 0.000000 +s off +f 1/1/1 2/2/2 3/3/3 +f 4/4/4 5/5/5 6/6/6 +f 7/7/7 8/8/8 9/9/9 +f 10/10/10 11/11/11 12/12/12 +f 13/13/13 5/5/5 12/12/12 +f 10/10/10 14/14/14 15/15/15 +f 16/16/16 1/1/1 3/3/3 +f 17/17/17 4/4/4 6/6/6 +f 18/18/18 7/7/7 9/9/9 +f 19/19/19 10/10/10 12/12/12 +f 20/20/20 13/13/13 12/12/12 +f 17/17/17 10/10/10 15/15/15 diff --git a/data/threecubes/three_cubes.mtl b/data/threecubes/three_cubes.mtl new file mode 100644 index 0000000000..e09a6c1397 --- /dev/null +++ b/data/threecubes/three_cubes.mtl @@ -0,0 +1,31 @@ +# Blender MTL File: 'None' +# Material Count: 3 + +newmtl Material +Ns 92.156863 +Ka 0.000000 0.000000 0.000000 +Kd 0.640000 0.000000 0.000000 +Ks 0.500000 0.500000 0.500000 +Ni 1.000000 +d 1.000000 +illum 2 + + +newmtl Material.001 +Ns 92.156863 +Ka 0.000000 0.000000 0.000000 +Kd 0.000000 0.640000 0.000000 +Ks 0.500000 0.500000 0.500000 +Ni 1.000000 +d 1.000000 +illum 2 + +newmtl Material.002 +Ns 92.156863 +Ka 0.000000 0.000000 0.000000 +Kd 0.000000 0.000000 0.640000 +Ks 0.500000 0.500000 0.500000 +Ni 1.000000 +d 1.000000 +illum 2 + diff --git a/data/threecubes/three_cubes.obj b/data/threecubes/three_cubes.obj new file mode 100644 index 0000000000..8fae6c02d2 --- /dev/null +++ b/data/threecubes/three_cubes.obj @@ -0,0 +1,84 @@ +# Blender v2.71 (sub 0) OBJ File: '' +# www.blender.org +mtllib three_cubes.mtl +o Cube.001 +v -4.474365 4.513343 1.511266 +v -4.474365 4.513344 -0.488734 +v -4.474365 2.513344 -0.488735 +v -4.474365 2.513344 1.511265 +v -2.474365 4.513344 -0.488734 +v -2.474365 2.513344 -0.488735 +v -2.474365 4.513343 1.511266 +v -2.474365 2.513344 1.511265 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 0.000000 0.000000 +usemtl Material.001 +s off +f 2/1 3/2 4/3 +f 5/1 6/2 3/3 +f 7/1 8/2 6/3 +f 1/1 4/2 8/3 +f 3/1 6/2 8/3 +f 1/1 7/2 5/3 +f 1/4 2/1 4/3 +f 2/4 5/1 3/3 +f 5/4 7/1 6/3 +f 7/4 1/1 8/3 +f 4/4 3/1 8/3 +f 2/4 1/1 5/3 +o Cube.002 +v -4.474365 4.513344 -1.663786 +v -4.474365 4.513344 -3.663786 +v -4.474365 2.513345 -3.663786 +v -4.474365 2.513344 -1.663787 +v -2.474365 4.513344 -3.663786 +v -2.474365 2.513345 -3.663786 +v -2.474365 4.513344 -1.663786 +v -2.474365 2.513344 -1.663787 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 0.000000 0.000000 +usemtl Material.002 +s off +f 10/5 11/6 12/7 +f 13/5 14/6 11/7 +f 15/5 16/6 14/7 +f 9/5 12/6 16/7 +f 11/5 14/6 16/7 +f 9/5 15/6 13/7 +f 9/8 10/5 12/7 +f 10/8 13/5 11/7 +f 13/8 15/5 14/7 +f 15/8 9/5 16/7 +f 12/8 11/5 16/7 +f 10/8 9/5 13/7 +o Cube +v -4.474365 4.513343 4.535691 +v -4.474365 4.513343 2.535691 +v -4.474365 2.513344 2.535691 +v -4.474365 2.513343 4.535690 +v -2.474365 4.513343 2.535691 +v -2.474365 2.513344 2.535691 +v -2.474365 4.513343 4.535691 +v -2.474365 2.513343 4.535690 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 0.000000 0.000000 +usemtl Material +s off +f 18/9 19/10 20/11 +f 21/9 22/10 19/11 +f 23/9 24/10 22/11 +f 17/9 20/10 24/11 +f 19/9 22/10 24/11 +f 17/9 23/10 21/11 +f 17/12 18/9 20/11 +f 18/12 21/9 19/11 +f 21/12 23/9 22/11 +f 23/12 17/9 24/11 +f 20/12 19/9 24/11 +f 18/12 17/9 21/11 diff --git a/data/threecubes/threecubes.py b/data/threecubes/threecubes.py new file mode 100644 index 0000000000..48efea514f --- /dev/null +++ b/data/threecubes/threecubes.py @@ -0,0 +1,8 @@ +import pybullet as p +p.connect(p.DIRECT) +p.loadPlugin("eglRendererPlugin") +p.loadSDF("newsdf.sdf") +while (1): + p.getCameraImage(320,240, flags=p.ER_NO_SEGMENTATION_MASK) + p.stepSimulation() + \ No newline at end of file diff --git a/data/threecubes/threecubes.urdf b/data/threecubes/threecubes.urdf new file mode 100644 index 0000000000..1729ef7645 --- /dev/null +++ b/data/threecubes/threecubes.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/threecubes/threecubes_org.py b/data/threecubes/threecubes_org.py new file mode 100644 index 0000000000..3b47f082fa --- /dev/null +++ b/data/threecubes/threecubes_org.py @@ -0,0 +1,22 @@ +import pybullet as p +useEGL = False +useEGLGUI = False + +if useEGL: + if useEGLGUI: + p.connect(p.GUI, "window_backend=2") + else: + p.connect(p.DIRECT) + p.loadPlugin("eglRendererPlugin") +else: + p.connect(p.GUI) + +p.loadURDF("threecubes.urdf", flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL) +while (1): + + viewmat= [0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574, 0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004, 0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0] + projmat= [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] + + p.getCameraImage(64,64, viewMatrix=viewmat, projectionMatrix=projmat, flags=p.ER_NO_SEGMENTATION_MASK ) + p.stepSimulation() + \ No newline at end of file diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index 4115effd56..5f553bcbee 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -53,6 +53,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& meshData.m_textureImage1 = 0; meshData.m_textureHeight = 0; meshData.m_textureWidth = 0; + meshData.m_flags = 0; meshData.m_isCached = false; char relativeFileName[1024]; @@ -78,6 +79,18 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& for (int i = 0; meshData.m_textureImage1 == 0 && i < shapes.size(); i++) { const tinyobj::shape_t& shape = shapes[i]; + meshData.m_rgbaColor[0] = shape.material.diffuse[0]; + meshData.m_rgbaColor[1] = shape.material.diffuse[1]; + meshData.m_rgbaColor[2] = shape.material.diffuse[2]; + meshData.m_rgbaColor[3] = shape.material.transparency; + meshData.m_flags |= B3_IMPORT_MESH_HAS_RGBA_COLOR; + + meshData.m_specularColor[0] = shape.material.specular[0]; + meshData.m_specularColor[1] = shape.material.specular[1]; + meshData.m_specularColor[2] = shape.material.specular[2]; + meshData.m_specularColor[3] = 1; + meshData.m_flags |= B3_IMPORT_MESH_HAS_SPECULAR_COLOR; + if (shape.material.diffuse_texname.length() > 0) { int width, height, n; diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h index 770eefd35b..1cff27b15c 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h @@ -3,6 +3,12 @@ #include +enum b3ImportMeshDataFlags +{ + B3_IMPORT_MESH_HAS_RGBA_COLOR=1, + B3_IMPORT_MESH_HAS_SPECULAR_COLOR=2, +}; + struct b3ImportMeshData { struct GLInstanceGraphicsShape* m_gfxShape; @@ -11,6 +17,20 @@ struct b3ImportMeshData bool m_isCached; int m_textureWidth; int m_textureHeight; + double m_rgbaColor[4]; + double m_specularColor[4]; + int m_flags; + + b3ImportMeshData() + :m_gfxShape(0), + m_textureImage1(0), + m_isCached(false), + m_textureWidth(0), + m_textureHeight(0), + m_flags(0) + { + } + }; class b3ImportMeshUtility diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 80f86b5e47..e867791746 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -228,24 +228,36 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase) } else { + char path[1024]; fu.extractPath(relativeFileName, path, sizeof(path)); m_data->setSourceFile(relativeFileName, path); - std::fstream xml_file(relativeFileName, std::fstream::in); - while (xml_file.good()) + //read file + int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r"); + + char destBuffer[8192]; + char* line = 0; + do { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); + line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192); + if (line) + { + xml_string += (std::string(destBuffer) + "\n"); + } } - xml_file.close(); + while (line); + m_data->m_fileIO->fileClose(fileId); } BulletErrorLogger loggie; //todo: quick test to see if we can re-use the URDF parser for SDF or not m_data->m_urdfParser.setParseSDF(true); - bool result = m_data->m_urdfParser.loadSDF(xml_string.c_str(), &loggie); + bool result = false; + if (xml_string.length()) + { + result = m_data->m_urdfParser.loadSDF(xml_string.c_str(), &loggie); + } return result; } @@ -861,7 +873,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl return shape; } -void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut) const +void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, struct b3ImportMeshData& meshData) const { BT_PROFILE("convertURDFToVisualShapeInternal"); @@ -920,7 +932,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu { case UrdfGeometry::FILE_OBJ: { - b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO)) { if (meshData.m_textureImage1) @@ -1150,16 +1162,49 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP btTransform childTrans = vis.m_linkLocalFrame; btHashString matName(vis.m_materialName.c_str()); UrdfMaterial* const* matPtr = model.m_materials[matName]; - if (matPtr) + b3ImportMeshData meshData; + + convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures,meshData); + + if (m_data->m_flags&CUF_USE_MATERIAL_COLORS_FROM_MTL) { - UrdfMaterial* const mat = *matPtr; - //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]); - UrdfMaterialColor matCol; - matCol.m_rgbaColor = mat->m_matColor.m_rgbaColor; - matCol.m_specularColor = mat->m_matColor.m_specularColor; - m_data->m_linkColors.insert(linkIndex, matCol); + if ((meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) && + (meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR)) + { + UrdfMaterialColor matCol; + + if (m_data->m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL) + { + matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], + meshData.m_rgbaColor[1], + meshData.m_rgbaColor[2], + meshData.m_rgbaColor[3]); + } else + { + matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], + meshData.m_rgbaColor[1], + meshData.m_rgbaColor[2], + 1); + } + + matCol.m_specularColor.setValue(meshData.m_specularColor[0], + meshData.m_specularColor[1], + meshData.m_specularColor[2]); + m_data->m_linkColors.insert(linkIndex, matCol); + } + } else + { + if (matPtr) + { + UrdfMaterial* const mat = *matPtr; + //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]); + UrdfMaterialColor matCol; + matCol.m_rgbaColor = mat->m_matColor.m_rgbaColor; + matCol.m_specularColor = mat->m_matColor.m_specularColor; + m_data->m_linkColors.insert(linkIndex, matCol); + } } - convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures); + } } if (vertices.size() && indices.size()) @@ -1266,6 +1311,7 @@ void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, UrdfLink* const* linkPtr = model.m_links.getAtIndex(urdfIndex); if (linkPtr) { + m_data->m_customVisualShapesConverter->setFlags(m_data->m_flags); m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, m_data->m_fileIO); } } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 8acf2cefd8..2dff82727e 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -88,7 +88,7 @@ class BulletURDFImporter : public URDFImporterInterface virtual int getAllocatedTexture(int index) const; virtual void setEnableTinyRenderer(bool enable); - void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut) const; + void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, struct b3ImportMeshData& meshData) const; }; #endif //BULLET_URDF_IMPORTER_H diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index f503a103e5..cbbd58b580 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -32,6 +32,8 @@ enum ConvertURDFFlags CUF_INITIALIZE_SAT_FEATURES = 4096, CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192, CUF_PARSE_SENSORS = 16384, + CUF_USE_MATERIAL_COLORS_FROM_MTL = 32768, + CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 64738, }; struct UrdfVisualShapeCache diff --git a/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h b/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h index 22913210ce..2903e7cf2b 100644 --- a/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h +++ b/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h @@ -74,16 +74,8 @@ static bool UrdfFindMeshFile( std::string existing_file; - { - std::string attempt = fn; - int f = fileIO->fileOpen(attempt.c_str(), "rb"); - if (f>=0) - { - existing_file = attempt; - fileIO->fileClose(f); - } - } - if (existing_file.empty()) + + { for (std::list::iterator x = shorter.begin(); x != shorter.end(); ++x) { @@ -100,6 +92,16 @@ static bool UrdfFindMeshFile( break; } } + if (existing_file.empty()) + { + std::string attempt = fn; + int f = fileIO->fileOpen(attempt.c_str(), "rb"); + if (f>=0) + { + existing_file = attempt; + fileIO->fileClose(f); + } + } if (existing_file.empty()) { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e7bcb6b075..1503b29bd2 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -9,7 +9,7 @@ #include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp" #include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h" - +#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h" #include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" @@ -2016,11 +2016,13 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface const b3CreateMultiBodyArgs& m_createBodyArgs; mutable b3AlignedObjectArray m_allocatedCollisionShapes; PhysicsServerCommandProcessorInternalData* m_data; + int m_flags; - ProgrammaticUrdfInterface(const b3CreateMultiBodyArgs& bodyArgs, PhysicsServerCommandProcessorInternalData* data) + ProgrammaticUrdfInterface(const b3CreateMultiBodyArgs& bodyArgs, PhysicsServerCommandProcessorInternalData* data, int flags) : m_bodyUniqueId(-1), m_createBodyArgs(bodyArgs), - m_data(data) + m_data(data), + m_flags(flags) { } @@ -2068,19 +2070,38 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface return false; } + mutable btHashMap m_linkColors; + virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const { - if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] >= 0) + + if (m_flags & URDF_USE_MATERIAL_COLORS_FROM_MTL) { - const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]); - if (visHandle) + const UrdfMaterialColor* matColPtr = m_linkColors[linkIndex]; + if (matColPtr) { - for (int i = 0; i < visHandle->m_visualShapes.size(); i++) + matCol = *matColPtr; + if ((m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL)==0) { - if (visHandle->m_visualShapes[i].m_geometry.m_hasLocalMaterial) + matCol.m_rgbaColor[3] = 1; + } + + return true; + } + } else + { + if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] >= 0) + { + const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]); + if (visHandle) + { + for (int i = 0; i < visHandle->m_visualShapes.size(); i++) { - matCol = visHandle->m_visualShapes[i].m_geometry.m_localMaterial.m_matColor; - return true; + if (visHandle->m_visualShapes[i].m_geometry.m_hasLocalMaterial) + { + matCol = visHandle->m_visualShapes[i].m_geometry.m_localMaterial.m_matColor; + return true; + } } } } @@ -2274,7 +2295,21 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface { for (int v = 0; v < visHandle->m_visualShapes.size(); v++) { - u2b.convertURDFToVisualShapeInternal(&visHandle->m_visualShapes[v], pathPrefix, localInertiaFrame.inverse() * visHandle->m_visualShapes[v].m_linkLocalFrame, vertices, indices, textures); + b3ImportMeshData meshData; + u2b.convertURDFToVisualShapeInternal(&visHandle->m_visualShapes[v], pathPrefix, localInertiaFrame.inverse() * visHandle->m_visualShapes[v].m_linkLocalFrame, vertices, indices, textures, meshData); + if ((meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) && + (meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR)) + { + UrdfMaterialColor matCol; + matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], + meshData.m_rgbaColor[1], + meshData.m_rgbaColor[2], + meshData.m_rgbaColor[3]); + matCol.m_specularColor.setValue(meshData.m_specularColor[0], + meshData.m_specularColor[1], + meshData.m_specularColor[2]); + m_linkColors.insert(linkIndex, matCol); + } } if (vertices.size() && indices.size()) @@ -3713,6 +3748,10 @@ bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struc m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false); } + if ((flags & ER_NO_SEGMENTATION_MASK) != 0) + { + segmentationMaskBuffer = 0; + } m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels, depthBuffer, numRequestedPixels, @@ -4462,6 +4501,7 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct } else { + visualShape.m_geometry.m_localMaterial.m_matColor.m_rgbaColor.setValue(1,1,1,1); } if (hasSpecular) { @@ -6641,21 +6681,23 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S { m_data->m_sdfRecentLoadedBodies.clear(); - ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data); + int flags = 0; - bool useMultiBody = true; - if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES) + if (clientCmd.m_updateFlags & MULT_BODY_HAS_FLAGS) { - useMultiBody = false; + flags = clientCmd.m_createMultiBodyArgs.m_flags; } - int flags = 0; + ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data, flags); - if (clientCmd.m_updateFlags & MULT_BODY_HAS_FLAGS) + bool useMultiBody = true; + if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES) { - flags = clientCmd.m_createMultiBodyArgs.m_flags; + useMultiBody = false; } + + bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); if (ok) diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 1cab59bf80..90ad43b762 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -819,6 +819,8 @@ enum eURDF_Flags URDF_INITIALIZE_SAT_FEATURES = 4096, URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192, URDF_PARSE_SENSORS = 16384, + URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768, + URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536, }; enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp index 4b06f48940..3ec0d1b27f 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp @@ -270,13 +270,14 @@ void EGLRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff) } ///todo: merge into single file with TinyRendererVisualShapeConverter -static void convertURDFToVisualShape2(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, b3VisualShapeData& visualShapeOut, struct CommonFileIOInterface* fileIO) +static void convertURDFToVisualShape2(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, b3VisualShapeData& visualShapeOut, struct CommonFileIOInterface* fileIO, int flags) { visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type; visualShapeOut.m_dimensions[0] = 0; visualShapeOut.m_dimensions[1] = 0; visualShapeOut.m_dimensions[2] = 0; memset(visualShapeOut.m_meshAssetFileName, 0, sizeof(visualShapeOut.m_meshAssetFileName)); +#if 0 if (visual->m_geometry.m_hasLocalMaterial) { visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[0]; @@ -284,6 +285,7 @@ static void convertURDFToVisualShape2(const UrdfShape* visual, const char* urdfP visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[2]; visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[3]; } +#endif GLInstanceGraphicsShape* glmesh = 0; @@ -412,6 +414,23 @@ static void convertURDFToVisualShape2(const UrdfShape* visual, const char* urdfP b3ImportMeshData meshData; if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, fileIO)) { + if (flags&URDF_USE_MATERIAL_COLORS_FROM_MTL) + { + if (meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) + { + visualShapeOut.m_rgbaColor[0] = meshData.m_rgbaColor[0]; + visualShapeOut.m_rgbaColor[1] = meshData.m_rgbaColor[1]; + visualShapeOut.m_rgbaColor[2] = meshData.m_rgbaColor[2]; + + if (flags&URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL) + { + visualShapeOut.m_rgbaColor[3] = meshData.m_rgbaColor[3]; + } else + { + visualShapeOut.m_rgbaColor[3] = 1; + } + } + } if (meshData.m_textureImage1) { MyTexture3 texData; @@ -752,7 +771,7 @@ void EGLRendererVisualShapeConverter::convertVisualShapes( visualShape.m_rgbaColor[3] = rgbaColor[3]; { B3_PROFILE("convertURDFToVisualShape2"); - convertURDFToVisualShape2(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO); + convertURDFToVisualShape2(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO, m_data->m_flags); } m_data->m_visualShapes.push_back(visualShape); diff --git a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp index 84e522cf04..c434113cac 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp +++ b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp @@ -6,8 +6,8 @@ #include #include "../../../CommonInterfaces/CommonFileIOInterface.h" #include "../../../Utils/b3ResourcePath.h" - - +#include "Bullet3Common/b3HashMap.h" +#include //memcpy/strlen #ifndef B3_EXCLUDE_DEFAULT_FILEIO #include "../../../Utils/b3BulletDefaultFileIO.h" #endif //B3_EXCLUDE_DEFAULT_FILEIO @@ -30,13 +30,258 @@ struct WrapperFileHandle int m_childFileHandle; }; +struct InMemoryFile +{ + char* m_buffer; + int m_fileSize; +}; + +struct InMemoryFileAccessor +{ + InMemoryFile* m_file; + int m_curPos; +}; + +struct InMemoryFileIO : public CommonFileIOInterface +{ + b3HashMap m_fileCache; + InMemoryFileAccessor m_fileHandles[B3_MAX_FILEIO_INTERFACES]; + + InMemoryFileIO() + { + for (int i=0;im_buffer); + delete (mem); + } + } + } + + char* allocateBuffer(int len) + { + char* buffer = 0; + if (len) + { + buffer = new char[len]; + } + return buffer; + } + + void freeBuffer(char* buffer) + { + delete[] buffer; + } + + virtual int registerFile(const char* fileName, char* buffer, int len) + { + InMemoryFile* f = new InMemoryFile(); + f->m_buffer = buffer; + f->m_fileSize = len; + b3HashString key(fileName); + m_fileCache.insert(key,f); + return 0; + } + + void removeFileFromCache(const char* fileName) + { + InMemoryFile* f = getInMemoryFile(fileName); + if (f) + { + m_fileCache.remove(fileName); + freeBuffer(f->m_buffer); + delete (f); + } + } + + InMemoryFile* getInMemoryFile(const char* fileName) + { + InMemoryFile** fPtr = m_fileCache[fileName]; + if (fPtr && *fPtr) + { + return *fPtr; + } + return 0; + } + + virtual int fileOpen(const char* fileName, const char* mode) + { + //search a free slot + int slot = -1; + for (int i=0;i=0) + { + InMemoryFile* f = getInMemoryFile(fileName); + if (f) + { + m_fileHandles[slot].m_curPos = 0; + m_fileHandles[slot].m_file = f; + } else + { + slot=-1; + } + } + //printf("InMemoryFileIO fileOpen %s, %d\n", fileName, slot); + return slot; + } + virtual int fileRead(int fileHandle, char* destBuffer, int numBytes) + { + if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES) + { + InMemoryFileAccessor& f = m_fileHandles[fileHandle]; + if (f.m_file) + { + //if (numBytes>1) + // printf("curPos = %d\n", f.m_curPos); + if (f.m_curPos+numBytes <= f.m_file->m_fileSize) + { + memcpy(destBuffer,f.m_file->m_buffer+f.m_curPos,numBytes); + f.m_curPos+=numBytes; + //if (numBytes>1) + // printf("read %d bytes, now curPos = %d\n", numBytes, f.m_curPos); + return numBytes; + } else + { + if (numBytes!=1) + { + printf("InMemoryFileIO::fileRead Attempt to read beyond end of file\n"); + } + } + + } + } + return 0; + } + + virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes) + { + return 0; + } + virtual void fileClose(int fileHandle) + { + if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES) + { + InMemoryFileAccessor& f = m_fileHandles[fileHandle]; + if (f.m_file) + { + m_fileHandles[fileHandle].m_file = 0; + m_fileHandles[fileHandle].m_curPos = 0; + //printf("InMemoryFileIO fileClose %d\n", fileHandle); + } + } + } + virtual bool findResourcePath(const char* fileName, char* resourcePathOut, int resourcePathMaxNumBytes) + { + InMemoryFile* f = getInMemoryFile(fileName); + int fileNameLen = strlen(fileName); + if (f && fileNameLen<(resourcePathMaxNumBytes-1)) + { + memcpy(resourcePathOut, fileName, fileNameLen); + resourcePathOut[fileNameLen]=0; + return true; + } + return false; + } + virtual char* readLine(int fileHandle, char* destBuffer, int numBytes) + { + int numRead = 0; + int endOfFile = 0; + if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES ) + { + InMemoryFileAccessor& f = m_fileHandles[fileHandle]; + if (f.m_file) + { + //return ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]); + char c = 0; + do + { + int bytesRead = fileRead(fileHandle,&c,1); + if (bytesRead != 1) + { + endOfFile = 1; + c=0; + } + if (c && c!='\n') + { + char a='\r'; + if (c!=13) + { + destBuffer[numRead++]=c; + } else + { + destBuffer[numRead++]=0; + } + } + } while (c != 0 && c != '\n' && numRead<(numBytes-1)); + } + } + if (numRead==0 && endOfFile) + { + return 0; + } + + if (numRead=0) + { + destBuffer[numRead]=0; + } + return &destBuffer[0]; + } else + { + if (endOfFile==0) + { + printf("InMemoryFileIO::readLine readLine warning: numRead=%d, numBytes=%d\n", numRead, numBytes); + } + } + return 0; + } + virtual int getFileSize(int fileHandle) + { + if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES ) + { + + InMemoryFileAccessor& f = m_fileHandles[fileHandle]; + if (f.m_file) + { + return f.m_file->m_fileSize; + } + } + return 0; + } +}; + struct WrapperFileIO : public CommonFileIOInterface { CommonFileIOInterface* m_availableFileIOInterfaces[B3_MAX_FILEIO_INTERFACES]; int m_numWrapperInterfaces; WrapperFileHandle m_wrapperFileHandles[B3_MAX_FILEIO_INTERFACES]; - + InMemoryFileIO m_cachedFiles; WrapperFileIO() :m_numWrapperInterfaces(0) @@ -47,6 +292,7 @@ struct WrapperFileIO : public CommonFileIOInterface m_wrapperFileHandles[i].childFileIO=0; m_wrapperFileHandles[i].m_childFileHandle=0; } + //addFileIOInterface(&m_cachedFiles); } virtual ~WrapperFileIO() @@ -86,6 +332,7 @@ struct WrapperFileIO : public CommonFileIOInterface virtual int fileOpen(const char* fileName, const char* mode) { + //find an available wrapperFileHandle slot int wrapperFileHandle=-1; int slot = -1; @@ -99,20 +346,76 @@ struct WrapperFileIO : public CommonFileIOInterface } if (slot>=0) { - //figure out what wrapper interface to use - //use the first one that can open the file - for (int i=0;ifileOpen(fileName, mode); - if (childHandle>=0) + CommonFileIOInterface* childFileIO=m_availableFileIOInterfaces[i]; + if (childFileIO) { - wrapperFileHandle = slot; - m_wrapperFileHandles[slot].childFileIO = childFileIO; - m_wrapperFileHandles[slot].m_childFileHandle = childHandle; - break; + int childHandle = childFileIO->fileOpen(fileName, mode); + if (childHandle>=0) + { + int fileSize = childFileIO->getFileSize(childHandle); + char* buffer = 0; + if (fileSize) + { + buffer = m_cachedFiles.allocateBuffer(fileSize); + if (buffer) + { + int readBytes = childFileIO->fileRead(childHandle, buffer, fileSize); + if (readBytes!=fileSize) + { + if (readBytesfileClose(childHandle); + break; + } + } + } + } + + { + int childHandle = m_cachedFiles.fileOpen(fileName, mode); + if (childHandle>=0) + { + wrapperFileHandle = slot; + m_wrapperFileHandles[slot].childFileIO = &m_cachedFiles; + m_wrapperFileHandles[slot].m_childFileHandle = childHandle; + } else + { + //figure out what wrapper interface to use + //use the first one that can open the file + for (int i=0;ifileOpen(fileName, mode); + if (childHandle>=0) + { + wrapperFileHandle = slot; + m_wrapperFileHandles[slot].childFileIO = childFileIO; + m_wrapperFileHandles[slot].m_childFileHandle = childHandle; + break; + } + } } } } @@ -155,6 +458,9 @@ struct WrapperFileIO : public CommonFileIOInterface } virtual bool findResourcePath(const char* fileName, char* resourcePathOut, int resourcePathMaxNumBytes) { + if (m_cachedFiles.findResourcePath(fileName, resourcePathOut, resourcePathMaxNumBytes)) + return true; + bool found = false; for (int i=0;im_text) { - obj->m_fileIO.addFileIOInterface(new ZipFileIO(arguments->m_text)); + obj->m_fileIO.addFileIOInterface(new ZipFileIO(arguments->m_text, &obj->m_fileIO)); } #else printf("eZipFileIO is not enabled in this build.\n"); diff --git a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h index 0df5770524..7366f1e248 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h +++ b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h @@ -10,7 +10,7 @@ struct ZipFileIO : public CommonFileIOInterface unzFile m_fileHandles[B3_ZIP_FILEIO_MAX_FILES ]; int m_numFileHandles; - ZipFileIO(const char* zipfileName) + ZipFileIO(const char* zipfileName, CommonFileIOInterface* wrapperFileIO) :m_zipfileName(zipfileName), m_numFileHandles(0) { @@ -37,6 +37,7 @@ struct ZipFileIO : public CommonFileIOInterface virtual int fileOpen(const char* fileName, const char* mode) { + //search a free slot int slot = -1; for (int i=0;i=0) @@ -82,20 +81,28 @@ struct ZipFileIO : public CommonFileIOInterface { printf("unzGetCurrentFileInfo() != UNZ_OK (%d)\n", result); slot=-1; + unzClose(zipfile); + zipfile = 0; } else { result = unzOpenCurrentFile(zipfile); if (result == UNZ_OK) { + printf("zipFile::fileOpen %s in mode %s in fileHandle %d\n", fileName, mode, slot); + m_fileHandles[slot] = zipfile; } else { slot=-1; + unzClose(zipfile); + zipfile = 0; } } } else { slot=-1; + unzClose(zipfile); + zipfile = 0; } } } @@ -137,6 +144,7 @@ struct ZipFileIO : public CommonFileIOInterface unzFile f = m_fileHandles[fileHandle]; if (f) { + printf("zipFile::fileClose slot %d\n", fileHandle); unzClose(f); m_fileHandles[fileHandle]=0; } diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 32cbd8fd48..a414758cfb 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -182,13 +182,14 @@ void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff m_data->m_hasLightSpecularCoeff = true; } -static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, b3VisualShapeData& visualShapeOut, struct CommonFileIOInterface* fileIO) +static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, b3VisualShapeData& visualShapeOut, struct CommonFileIOInterface* fileIO, int flags) { visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type; visualShapeOut.m_dimensions[0] = 0; visualShapeOut.m_dimensions[1] = 0; visualShapeOut.m_dimensions[2] = 0; memset(visualShapeOut.m_meshAssetFileName, 0, sizeof(visualShapeOut.m_meshAssetFileName)); +#if 0 if (visual->m_geometry.m_hasLocalMaterial) { visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[0]; @@ -196,6 +197,7 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[2]; visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[3]; } +#endif GLInstanceGraphicsShape* glmesh = 0; @@ -322,8 +324,26 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa { //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, fileIO)) { + if (flags&URDF_USE_MATERIAL_COLORS_FROM_MTL) + { + if (meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) + { + visualShapeOut.m_rgbaColor[0] = meshData.m_rgbaColor[0]; + visualShapeOut.m_rgbaColor[1] = meshData.m_rgbaColor[1]; + visualShapeOut.m_rgbaColor[2] = meshData.m_rgbaColor[2]; + + if (flags&URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL) + { + visualShapeOut.m_rgbaColor[3] = meshData.m_rgbaColor[3]; + } else + { + visualShapeOut.m_rgbaColor[3] = 1; + } + } + } if (meshData.m_textureImage1) { MyTexture2 texData; @@ -665,9 +685,14 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( { B3_PROFILE("convertURDFToVisualShape"); - convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO); + convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO, m_data->m_flags); } + rgbaColor[0] = visualShape.m_rgbaColor[0]; + rgbaColor[1] = visualShape.m_rgbaColor[1]; + rgbaColor[2] = visualShape.m_rgbaColor[2]; + rgbaColor[3] = visualShape.m_rgbaColor[3]; + if (vertices.size() && indices.size()) { TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer, m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex); diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp index ebc9866473..115cde02a5 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp @@ -407,7 +407,7 @@ std::string LoadMtl( } if (linebuf.size() > 0) { - if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1); + if (linebuf[linebuf.size() - 1] == '\r') linebuf.erase(linebuf.size() - 1); } // Skip if empty line. @@ -417,11 +417,10 @@ std::string LoadMtl( } linebuf = linebuf.substr(0, linebuf.find_last_not_of(" \t") + 1); - // Skip leading space. const char* token = linebuf.c_str(); token += strspn(token, " \t"); - + assert(token); if (token[0] == '\0') continue; // empty line diff --git a/examples/Utils/b3BulletDefaultFileIO.h b/examples/Utils/b3BulletDefaultFileIO.h index 927675e1ab..6d681bbb0d 100644 --- a/examples/Utils/b3BulletDefaultFileIO.h +++ b/examples/Utils/b3BulletDefaultFileIO.h @@ -148,7 +148,16 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface FILE* f = m_fileHandles[fileHandle]; if (f) { - return ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]); + char* txt = ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]); + for (int i=0;i= v0.9.x # for extension of this class. def step(self, *args, **kwargs): + if self.isRender: + base_pos=[0,0,0] + if (hasattr(self,'robot')): + if (hasattr(self.robot,'body_xyz')): + base_pos = self.robot.body_xyz + # Keep the previous orientation of the camera set by the user. + #[yaw, pitch, dist] = self._p.getDebugVisualizerCamera()[8:11] + self._p.resetDebugVisualizerCamera(3,0,0, base_pos) + + return self._step(*args, **kwargs) if parse_version(gym.__version__)>=parse_version('0.9.6'): diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py index 56461c13bc..8e256284cf 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py @@ -271,7 +271,7 @@ def _reset(self, initial_motor_angles=None, reset_duration=1.0): "%s/plane.urdf" % self._urdf_root) if (self._reflection): self._pybullet_client.changeVisualShape(self._ground_id,-1,rgbaColor=[1,1,1,0.8]) - #self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_PLANAR_REFLECTION,1) + self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_PLANAR_REFLECTION,self._ground_id) self._pybullet_client.setGravity(0, 0, -10) acc_motor = self._accurate_motor_model_enabled motor_protect = self._motor_overheat_protection diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e0f2383817..a98e2eca77 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -9850,7 +9850,9 @@ initpybullet(void) PyModule_AddIntConstant(m, "URDF_ENABLE_CACHED_GRAPHICS_SHAPES", URDF_ENABLE_CACHED_GRAPHICS_SHAPES); PyModule_AddIntConstant(m, "URDF_ENABLE_SLEEPING", URDF_ENABLE_SLEEPING); PyModule_AddIntConstant(m, "URDF_INITIALIZE_SAT_FEATURES", URDF_INITIALIZE_SAT_FEATURES); - + PyModule_AddIntConstant(m, "URDF_USE_MATERIAL_COLORS_FROM_MTL", URDF_USE_MATERIAL_COLORS_FROM_MTL); + PyModule_AddIntConstant(m, "URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL", URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL); + PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_SLEEPING", eActivationStateEnableSleeping); PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping); PyModule_AddIntConstant(m, "ACTIVATION_STATE_WAKE_UP", eActivationStateWakeUp);