diff --git a/src/systems/collada_world_exporter/ColladaWorldExporter.cc b/src/systems/collada_world_exporter/ColladaWorldExporter.cc index b8bf9faef1..69e86077cc 100644 --- a/src/systems/collada_world_exporter/ColladaWorldExporter.cc +++ b/src/systems/collada_world_exporter/ColladaWorldExporter.cc @@ -112,12 +112,27 @@ class ignition::gazebo::systems::ColladaWorldExporterPrivate ignition::common::MeshManager *meshManager = ignition::common::MeshManager::Instance(); - auto addSubmeshFunc = [&](int i, int k) { - subm = worldMesh.AddSubMesh( - *mesh->SubMeshByIndex(k).lock().get()); - subm.lock()->SetMaterialIndex(i); - subm.lock()->Scale(scale); - subMeshMatrix.push_back(matrix); + auto addSubmeshFunc = [&](int _matIndex) + { + int newMatIndex = 0; + if (_matIndex != -1) + { + newMatIndex = worldMesh.IndexOfMaterial( + mesh->MaterialByIndex(_matIndex).get()); + if (_matIndex < 0) + { + newMatIndex = worldMesh.AddMaterial( + mesh->MaterialByIndex(_matIndex)); + } + } + else + { + newMatIndex = worldMesh.AddMaterial(mat); + } + + subm.lock()->SetMaterialIndex(newMatIndex); + subm.lock()->Scale(scale); + subMeshMatrix.push_back(matrix); }; if (_geom->Data().Type() == sdf::GeometryType::BOX) @@ -126,9 +141,10 @@ class ignition::gazebo::systems::ColladaWorldExporterPrivate { mesh = meshManager->MeshByName("unit_box"); scale = _geom->Data().BoxShape()->Size(); - int i = worldMesh.AddMaterial(mat); + subm = worldMesh.AddSubMesh( + *mesh->SubMeshByIndex(0).lock().get()); - addSubmeshFunc(i, 0); + addSubmeshFunc(-1); } } else if (_geom->Data().Type() == sdf::GeometryType::CYLINDER) @@ -139,10 +155,10 @@ class ignition::gazebo::systems::ColladaWorldExporterPrivate scale.X() = _geom->Data().CylinderShape()->Radius() * 2; scale.Y() = scale.X(); scale.Z() = _geom->Data().CylinderShape()->Length(); + subm = worldMesh.AddSubMesh( + *mesh->SubMeshByIndex(0).lock().get()); - int i = worldMesh.AddMaterial(mat); - - addSubmeshFunc(i, 0); + addSubmeshFunc(-1); } } else if (_geom->Data().Type() == sdf::GeometryType::PLANE) @@ -164,9 +180,10 @@ class ignition::gazebo::systems::ColladaWorldExporterPrivate worldPose.Rot() = worldPose.Rot() * normalRot; matrix = math::Matrix4d(worldPose); + subm = worldMesh.AddSubMesh( + *mesh->SubMeshByIndex(0).lock().get()); - int i = worldMesh.AddMaterial(mat); - addSubmeshFunc(i, 0); + addSubmeshFunc(-1); } } else if (_geom->Data().Type() == sdf::GeometryType::SPHERE) @@ -179,8 +196,10 @@ class ignition::gazebo::systems::ColladaWorldExporterPrivate scale.Y() = scale.X(); scale.Z() = scale.X(); - int i = worldMesh.AddMaterial(mat); - addSubmeshFunc(i, 0); + subm = worldMesh.AddSubMesh( + *mesh->SubMeshByIndex(0).lock().get()); + + addSubmeshFunc(-1); } } else if (_geom->Data().Type() == sdf::GeometryType::MESH) @@ -200,27 +219,22 @@ class ignition::gazebo::systems::ColladaWorldExporterPrivate return true; } - for (unsigned int k = 0; k < mesh->SubMeshCount(); k++) + const auto subMeshName = _geom->Data().MeshShape()->Submesh(); + scale = _geom->Data().MeshShape()->Scale(); + if(subMeshName == "") { - auto subMeshLock = mesh->SubMeshByIndex(k).lock(); - int j = subMeshLock->MaterialIndex(); - - int i = 0; - if (j != -1) + for (unsigned int k = 0; k < mesh->SubMeshCount(); k++) { - i = worldMesh.IndexOfMaterial(mesh->MaterialByIndex(j).get()); - if (i < 0) - { - i = worldMesh.AddMaterial(mesh->MaterialByIndex(j)); - } + auto subMeshLock = mesh->SubMeshByIndex(k).lock(); + subm = worldMesh.AddSubMesh(*subMeshLock.get()); + addSubmeshFunc(subMeshLock->MaterialIndex()); } - else - { - i = worldMesh.AddMaterial(mat); - } - - scale = _geom->Data().MeshShape()->Scale(); - addSubmeshFunc(i, k); + } + else + { + auto subMeshLock = mesh->SubMeshByName(subMeshName).lock(); + subm = worldMesh.AddSubMesh(*subMeshLock.get()); + addSubmeshFunc(subMeshLock->MaterialIndex()); } } else diff --git a/test/integration/collada_world_exporter.cc b/test/integration/collada_world_exporter.cc index ffd4a1d7be..bfb54b600c 100644 --- a/test/integration/collada_world_exporter.cc +++ b/test/integration/collada_world_exporter.cc @@ -17,8 +17,11 @@ #include +#include #include #include +#include +#include #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/test_config.hh" @@ -95,6 +98,14 @@ TEST_F(ColladaWorldExporterFixture, ExportWorldFromFuelWithSubmesh) // The export directory should now exist. EXPECT_TRUE(common::exists(outputPath)); + // Original .dae file has two submeshes + // .sdf loads them together and a submesh alone + // Check that output has three nodes + common::ColladaLoader loader; + const common::Mesh *meshExported = loader.Load(common::joinPaths( + outputPath, "meshes", "collada_world_exporter_submesh_test.dae")); + EXPECT_EQ(3u, meshExported->SubMeshCount()); + // Cleanup common::removeAll(outputPath); } diff --git a/test/worlds/models/mesh_with_submeshes/model.sdf b/test/worlds/models/mesh_with_submeshes/model.sdf index 4bba6e8d75..973c4a7bf3 100644 --- a/test/worlds/models/mesh_with_submeshes/model.sdf +++ b/test/worlds/models/mesh_with_submeshes/model.sdf @@ -3,6 +3,17 @@ 0 0 1 0 0 0 + + + + model://mesh_with_submeshes/meshes/mesh_with_submeshes.dae + + node_0 +
false
+
+
+
+