From ae68e812466cbd580c0e7707b557e55c3d10e965 Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@openrobotics.org>
Date: Fri, 31 May 2024 00:55:15 +0900
Subject: [PATCH] Support mesh optimization when using AttachMeshShapeFeature
 (#2417)

Signed-off-by: Ian Chen <ichen@openrobotics.org>
---
 include/gz/sim/Util.hh |  7 ++++
 src/Util.cc            | 78 ++++++++++++++++++++++++++++++++++++++++++
 src/Util_TEST.cc       |  8 +++++
 3 files changed, 93 insertions(+)

diff --git a/include/gz/sim/Util.hh b/include/gz/sim/Util.hh
index e8e4bed0e0..de0585e5c0 100644
--- a/include/gz/sim/Util.hh
+++ b/include/gz/sim/Util.hh
@@ -316,6 +316,13 @@ namespace gz
     /// \return The loaded mesh or null if the mesh can not be loaded.
     GZ_SIM_VISIBLE const common::Mesh *loadMesh(const sdf::Mesh &_meshSdf);
 
+    /// \brief Optimize input mesh.
+    /// \param[in] _meshSdf Mesh SDF DOM with mesh optimization parameters
+    /// \param[in] _mesh Input mesh to optimize.
+    /// \return The optimized mesh or null if the mesh can not be optimized.
+    GZ_SIM_VISIBLE const common::Mesh *optimizeMesh(const sdf::Mesh &_meshSdf,
+        const common::Mesh &_mesh);
+
     /// \brief Environment variable holding resource paths.
     const std::string kResourcePathEnv{"GZ_SIM_RESOURCE_PATH"};
 
diff --git a/src/Util.cc b/src/Util.cc
index 8adfedbee7..e9cb2406f0 100644
--- a/src/Util.cc
+++ b/src/Util.cc
@@ -15,12 +15,17 @@
  *
 */
 
+#include <cstddef>
+#include <string>
+#include <vector>
+
 #include <gz/msgs/entity.pb.h>
 
 #include <gz/common/Filesystem.hh>
 #include <gz/common/Mesh.hh>
 #include <gz/common/MeshManager.hh>
 #include <gz/common/StringUtils.hh>
+#include <gz/common/SubMesh.hh>
 #include <gz/common/URI.hh>
 #include <gz/common/Util.hh>
 #include <gz/math/Helpers.hh>
@@ -862,8 +867,81 @@ const common::Mesh *loadMesh(const sdf::Mesh &_meshSdf)
            << "]." << std::endl;
     return nullptr;
   }
+
+  if (mesh && _meshSdf.Optimization() != sdf::MeshOptimization::NONE)
+  {
+    const common::Mesh *optimizedMesh = optimizeMesh(_meshSdf, *mesh);
+    if (optimizedMesh)
+      return optimizedMesh;
+    else
+      gzwarn << "Failed to optimize Mesh " << mesh->Name() << std::endl;
+  }
+
   return mesh;
 }
+
+const common::Mesh *optimizeMesh(const sdf::Mesh &_meshSdf,
+    const common::Mesh &_mesh)
+{
+  if (_meshSdf.Optimization() !=
+      sdf::MeshOptimization::CONVEX_DECOMPOSITION &&
+      _meshSdf.Optimization() !=
+      sdf::MeshOptimization::CONVEX_HULL)
+    return nullptr;
+
+  auto &meshManager = *common::MeshManager::Instance();
+  std::size_t maxConvexHulls = 16u;
+  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
+    maxConvexHulls = _meshSdf.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 =
+      _mesh.Name() + "_CONVEX_" + std::to_string(maxConvexHulls);
+  auto *optimizedMesh = meshManager.MeshByName(convexMeshName);
+  if (!optimizedMesh)
+  {
+    // Merge meshes before convex decomposition
+    auto mergedMesh = gz::common::MeshManager::MergeSubMeshes(_mesh);
+    if (mergedMesh && mergedMesh->SubMeshCount() == 1u)
+    {
+      // Decompose and add mesh to MeshManager
+      auto mergedSubmesh = mergedMesh->SubMeshByIndex(0u).lock();
+      std::vector<common::SubMesh> decomposed =
+        gz::common::MeshManager::ConvexDecomposition(
+        *mergedSubmesh.get(), maxConvexHulls);
+      gzdbg << "Optimizing mesh (" << _meshSdf.OptimizationStr() << "): "
+            <<  _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: "
+               << _mesh.Name() << std::endl;
+      }
+      optimizedMesh = meshManager.MeshByName(convexMeshName);
+    }
+  }
+  return optimizedMesh;
+}
+
 }
 }
 }
diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc
index 6be8d27903..9ec0e9be1a 100644
--- a/src/Util_TEST.cc
+++ b/src/Util_TEST.cc
@@ -1023,4 +1023,12 @@ TEST_F(UtilTest, LoadMesh)
     "test", "media", "duck.dae");
   meshSdf.SetFilePath(filePath);
   EXPECT_NE(nullptr, loadMesh(meshSdf));
+
+  EXPECT_TRUE(meshSdf.SetOptimization("convex_decomposition"));
+  sdf::ConvexDecomposition convexDecomp;
+  convexDecomp.SetMaxConvexHulls(16u);
+  meshSdf.SetConvexDecomposition(convexDecomp);
+  auto *optimizedMesh = loadMesh(meshSdf);
+  EXPECT_NE(nullptr, optimizedMesh);
+  EXPECT_EQ(16u, optimizedMesh->SubMeshCount());
 }