Extend optimizeMesh take into account voxelResolution and submesh par…

…ams, refactor visualizaiton capabilities to use optimizeMesh function

Signed-off-by: Ian Chen <[email protected]>

src/Util.cc

@@ -891,32 +891,60 @@ const common::Mesh *optimizeMesh(const sdf::Mesh &_meshSdf,
 
   auto &meshManager = *common::MeshManager::Instance();
   std::size_t maxConvexHulls = 16u;
+  std::size_t voxelResolution = 200000u;
   if (_meshSdf.Optimization() == sdf::MeshOptimization::CONVEX_HULL)
   {
     /// create 1 convex hull for the whole submesh
     maxConvexHulls = 1u;
   }
   else if (_meshSdf.ConvexDecomposition())
   {
-    // limit max number of convex hulls to generate
+    // set convex decomposition params: max number of convex hulls
+    // and voxel resolution
     maxConvexHulls = _meshSdf.ConvexDecomposition()->MaxConvexHulls();
+    voxelResolution = _meshSdf.ConvexDecomposition()->VoxelResolution();
   }
   // Check if MeshManager contains the decomposed mesh already. If not
   // add it to the MeshManager so we do not need to decompose it again.
   const std::string convexMeshName =
-      _mesh.Name() + "_CONVEX_" + std::to_string(maxConvexHulls);
+      _mesh.Name() + "_" + _meshSdf.Submesh() + "_CONVEX_" +
+      std::to_string(maxConvexHulls) + "_" + std::to_string(voxelResolution);
   auto *optimizedMesh = meshManager.MeshByName(convexMeshName);
   if (!optimizedMesh)
   {
-    // Merge meshes before convex decomposition
-    auto mergedMesh = gz::common::MeshManager::MergeSubMeshes(_mesh);
-    if (mergedMesh && mergedMesh->SubMeshCount() == 1u)
+    std::unique_ptr<common::Mesh> meshToDecompose =
+        std::make_unique<common::Mesh>();
+    // check if a particular submesh is requested
+    if (!_meshSdf.Submesh().empty())
+    {
+      for (unsigned int submeshIdx = 0;
+           submeshIdx < _mesh.SubMeshCount();
+           ++submeshIdx)
+      {
+        auto submesh = _mesh.SubMeshByIndex(submeshIdx).lock();
+        if (submesh->Name() == _meshSdf.Submesh())
+        {
+          if (_meshSdf.CenterSubmesh())
+            submesh->Center(math::Vector3d::Zero);
+          meshToDecompose->AddSubMesh(*submesh.get());
+          break;
+        }
+      }
+    }
+    else
+    {
+      // Merge meshes before convex decomposition
+      meshToDecompose =
+           gz::common::MeshManager::MergeSubMeshes(_mesh);
+    }
+
+    if (meshToDecompose && meshToDecompose->SubMeshCount() == 1u)
     {
       // Decompose and add mesh to MeshManager
-      auto mergedSubmesh = mergedMesh->SubMeshByIndex(0u).lock();
+      auto mergedSubmesh = meshToDecompose->SubMeshByIndex(0u).lock();
       std::vector<common::SubMesh> decomposed =
         gz::common::MeshManager::ConvexDecomposition(
-        *mergedSubmesh.get(), maxConvexHulls);
+        *mergedSubmesh.get(), maxConvexHulls, voxelResolution);
       gzdbg << "Optimizing mesh (" << _meshSdf.OptimizationStr() << "): "
             <<  _mesh.Name() << std::endl;
       // Create decomposed mesh and add it to MeshManager

src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc

@@ -1241,100 +1241,20 @@ rendering::GeometryPtr VisualizationCapabilitiesPrivate::CreateGeometry(
 
     // Assume absolute path to mesh file
     descriptor.meshName = fullPath;
-    descriptor.subMeshName = _geom.MeshShape()->Submesh();
-    descriptor.centerSubMesh = _geom.MeshShape()->CenterSubmesh();
-
     gz::common::MeshManager *meshManager =
         gz::common::MeshManager::Instance();
     descriptor.mesh = meshManager->Load(descriptor.meshName);
     if (descriptor.mesh)
     {
-      if (_geom.MeshShape()->Optimization() ==
-          sdf::MeshOptimization::CONVEX_HULL ||
-          _geom.MeshShape()->Optimization() ==
-          sdf::MeshOptimization::CONVEX_DECOMPOSITION)
+      if (_geom.MeshShape()->Optimization() != sdf::MeshOptimization::NONE)
       {
-        std::size_t maxConvexHulls = 16u;
-        if (_geom.MeshShape()->Optimization() ==
-            sdf::MeshOptimization::CONVEX_HULL)
-        {
-          /// create 1 convex hull for the whole submesh
-          maxConvexHulls = 1u;
-        }
-        else if (_geom.MeshShape()->ConvexDecomposition())
-        {
-          // limit max number of convex hulls to generate
-          maxConvexHulls =
-              _geom.MeshShape()->ConvexDecomposition()->MaxConvexHulls();
-        }
-
-        // Check if MeshManager contains the decomposed mesh already. If not
-        // add it to the MeshManager so we do not need to decompose it again.
-        const std::string convexMeshName =
-            descriptor.mesh->Name() + "_" + descriptor.subMeshName +
-            "_CONVEX_" + std::to_string(maxConvexHulls);
-        auto *decomposedMesh = meshManager->MeshByName(convexMeshName);
-        if (!decomposedMesh)
-        {
-          std::unique_ptr<common::Mesh> meshToDecompose =
-              std::make_unique<common::Mesh>();
-          // check if a particular submesh is requested
-          if (!descriptor.subMeshName.empty())
-          {
-            for (unsigned int submeshIdx = 0;
-                 submeshIdx < descriptor.mesh->SubMeshCount();
-                 ++submeshIdx)
-            {
-              auto submesh = descriptor.mesh->SubMeshByIndex(submeshIdx).lock();
-              if (submesh->Name() == descriptor.subMeshName)
-              {
-                if (descriptor.centerSubMesh)
-                  submesh->Center(math::Vector3d::Zero);
-                meshToDecompose->AddSubMesh(*submesh.get());
-                break;
-              }
-            }
-          }
-          else
-          {
-            meshToDecompose =
-                gz::common::MeshManager::MergeSubMeshes(*descriptor.mesh);
-          }
-          if (meshToDecompose && meshToDecompose->SubMeshCount() == 1u)
-          {
-            // Decompose and add mesh to MeshManager
-            auto s = meshToDecompose->SubMeshByIndex(0u).lock();
-            std::vector<common::SubMesh> decomposed =
-                gz::common::MeshManager::ConvexDecomposition(
-                *s.get(), maxConvexHulls);
-            gzdbg << "Optimizing mesh (" << _geom.MeshShape()->OptimizationStr()
-                  << "): " <<  descriptor.mesh->Name() << std::endl;
-
-            // Create decomposed mesh and add it to MeshManager
-            // Note: MeshManager will call delete on this mesh in its destructor
-            // \todo(iche033) Consider updating MeshManager to accept
-            // unique pointers instead
-            common::Mesh *convexMesh = new common::Mesh;
-            convexMesh->SetName(convexMeshName);
-            for (const auto & submesh : decomposed)
-              convexMesh->AddSubMesh(submesh);
-            meshManager->AddMesh(convexMesh);
-            if (decomposed.empty())
-            {
-              // Print an error if convex decomposition returned empty submeshes
-              // but still add it to MeshManager to avoid going through the
-              // expensive convex decomposition process for the same mesh again
-              gzerr << "Convex decomposition generated zero meshes: "
-                     << descriptor.mesh->Name() << std::endl;
-            }
-            decomposedMesh = meshManager->MeshByName(convexMeshName);
-          }
-        }
-        if (decomposedMesh)
+        const common::Mesh *optimizedMesh =
+            optimizeMesh(*_geom.MeshShape(), *descriptor.mesh);
+        if (optimizedMesh)
         {
-          descriptor.mesh = decomposedMesh;
-          // if submesh is requested, we handled this above before mesh
-          // decomposition so we do not need need to pass these flags to
+          descriptor.mesh = optimizedMesh;
+          // if submesh is requested, it should be handled in the optimizeMesh
+          // function so we do not need need to pass these flags to
           // gz-rendering
           descriptor.subMeshName = "";
           descriptor.centerSubMesh = false;