diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 9add4bddc..bf2e74ed2 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -28,7 +28,7 @@ subject to the following restrictions: #include "../../Utils/b3ResourcePath.h" #include "../../Utils/b3BulletDefaultFileIO.h" - +#include "../OpenGLWindow/ShapeData.h" #include "URDF2Bullet.h" //for flags #include "../ImportMeshUtility/b3ImportMeshUtility.h" @@ -919,10 +919,40 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu case URDF_GEOM_BOX: { btVector3 extents = visual->m_geometry.m_boxSize; - btBoxShape* boxShape = new btBoxShape(extents * 0.5f); - //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); - convexColShape = boxShape; - convexColShape->setMargin(gUrdfDefaultCollisionMargin); + int strideInBytes = 9 * sizeof(float); + int numVertices = sizeof(cube_vertices_textured) / strideInBytes; + int numIndices = sizeof(cube_indices) / sizeof(int); + glmesh = new GLInstanceGraphicsShape; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + glmesh->m_indices->resize(numIndices); + for (int k = 0; k < numIndices; k++) + { + glmesh->m_indices->at(k) = cube_indices[k]; + } + glmesh->m_vertices->resize(numVertices); + + btScalar halfExtentsX = extents[0] * 0.5; + btScalar halfExtentsY = extents[1] * 0.5; + btScalar halfExtentsZ = extents[2] * 0.5; + GLInstanceVertex* verts = &glmesh->m_vertices->at(0); + btScalar textureScaling = 1; + + for (int i = 0; i < numVertices; i++) + { + verts[i].xyzw[0] = halfExtentsX * cube_vertices_textured[i * 9]; + verts[i].xyzw[1] = halfExtentsY * cube_vertices_textured[i * 9 + 1]; + verts[i].xyzw[2] = halfExtentsZ * cube_vertices_textured[i * 9 + 2]; + verts[i].xyzw[3] = cube_vertices_textured[i * 9 + 3]; + verts[i].normal[0] = cube_vertices_textured[i * 9 + 4]; + verts[i].normal[1] = cube_vertices_textured[i * 9 + 5]; + verts[i].normal[2] = cube_vertices_textured[i * 9 + 6]; + verts[i].uv[0] = cube_vertices_textured[i * 9 + 7] * textureScaling; + verts[i].uv[1] = cube_vertices_textured[i * 9 + 8] * textureScaling; + } + + glmesh->m_numIndices = numIndices; + glmesh->m_numvertices = numVertices; break; } @@ -1102,8 +1132,10 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu vtx.normal[0] = pos.x(); vtx.normal[1] = pos.y(); vtx.normal[2] = pos.z(); - vtx.uv[0] = 0.5f; - vtx.uv[1] = 0.5f; + btScalar u = btAtan2(vtx.normal[0], vtx.normal[2]) / (2 * SIMD_PI) + 0.5; + btScalar v = vtx.normal[1] * 0.5 + 0.5; + vtx.uv[0] = u; + vtx.uv[1] = v; glmesh->m_vertices->push_back(vtx); } diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 399725ab2..e3b5d7992 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -32,7 +32,7 @@ subject to the following restrictions: #include "../SharedMemory/SharedMemoryPublic.h" //for b3VisualShapeData #include "../TinyRenderer/model.h" #include "stb_image/stb_image.h" - +#include "../OpenGLWindow/ShapeData.h" struct MyTexture2 { unsigned char* textureData1; @@ -293,10 +293,47 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa btVector3 extents = visual->m_geometry.m_boxSize; - btBoxShape* boxShape = new btBoxShape(extents * 0.5f); - //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); - convexColShape = boxShape; - convexColShape->setMargin(0.001); + + int strideInBytes = 9 * sizeof(float); + int numVertices = sizeof(cube_vertices_textured) / strideInBytes; + int numIndices = sizeof(cube_indices) / sizeof(int); + + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + glmesh->m_indices->resize(numIndices); + for (int k = 0; k < numIndices; k++) + { + glmesh->m_indices->at(k) = cube_indices[k]; + } + glmesh->m_vertices->resize(numVertices); + + btScalar halfExtentsX = extents[0] * 0.5; + btScalar halfExtentsY = extents[1] * 0.5; + btScalar halfExtentsZ = extents[2] * 0.5; + GLInstanceVertex* verts = &glmesh->m_vertices->at(0); + btScalar textureScaling = 1; + + for (int i = 0; i < numVertices; i++) + { + + verts[i].xyzw[0] = halfExtentsX * cube_vertices_textured[i * 9]; + verts[i].xyzw[1] = halfExtentsY * cube_vertices_textured[i * 9 + 1]; + verts[i].xyzw[2] = halfExtentsZ * cube_vertices_textured[i * 9 + 2]; + verts[i].xyzw[3] = cube_vertices_textured[i * 9 + 3]; + verts[i].normal[0] = cube_vertices_textured[i * 9 + 4]; + verts[i].normal[1] = cube_vertices_textured[i * 9 + 5]; + verts[i].normal[2] = cube_vertices_textured[i * 9 + 6]; + verts[i].uv[0] = cube_vertices_textured[i * 9 + 7] * textureScaling; + verts[i].uv[1] = cube_vertices_textured[i * 9 + 8] * textureScaling; + } + + glmesh->m_numIndices = numIndices; + glmesh->m_numvertices = numVertices; + + break; } case URDF_GEOM_SPHERE: