Add method for doing convex decomposition

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

graphics/include/gz/common/MeshManager.hh

@@ -280,6 +280,21 @@ namespace gz
           const Mesh *_m2, const int _operation,
           const gz::math::Pose3d &_offset = gz::math::Pose3d::Zero);
 
+      /// \brief Perform convex decomposition on a submesh.
+      /// The submesh is decomposed into multiple convex submeshes. The output
+      /// submeshes contain vertices and indices but texture coordinates
+      /// not preserved.
+      /// \param[in] _subMesh Input submesh to decompose.
+      /// \param[in] _maxConvexHulls Maximum number of convex hull submeshes to
+      /// produce.
+      /// \param[in] _voxelResolution Voxel resolution to use. Higher value
+      /// produces more accurate shapes.
+      /// \return A vector of decomposed submeshes.
+      public: std::vector<SubMesh> ConvexDecomposition(
+                  const common::SubMesh *_subMesh,
+                  std::size_t _maxConvexHulls = 16u,
+                  std::size_t _voxelResolution = 200000u);
+
       /// \brief Converts a vector of polylines into a table of vertices and
       /// a list of edges (each made of 2 points from the table of vertices.
       /// \param[in] _polys the polylines

graphics/src/MeshManager.cc

@@ -22,6 +22,14 @@
 #include <unordered_set>
 #include <cctype>
 
+// Suppress warnings for VHACD
+#pragma GCC diagnostic ignored "-Wfloat-equal"
+#pragma GCC diagnostic ignored "-Wswitch-default"
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#define ENABLE_VHACD_IMPLEMENTATION 1
+#include "VHACD.h"
+#pragma GCC diagnostic pop
+
 #ifndef _WIN32
   #include "gz/common/GTSMeshUtils.hh"
   #include "gz/common/MeshCSG.hh"
@@ -1649,3 +1657,82 @@ void MeshManager::SetAssimpEnvs()
     this->dataPtr->forceAssimp = true;
   }
 }
+
+//////////////////////////////////////////////////
+std::vector<SubMesh>
+MeshManager::ConvexDecomposition(const SubMesh *_subMesh,
+                                 std::size_t _maxConvexHulls,
+                                 std::size_t _voxelResolution)
+{
+  std::vector<SubMesh> decomposed;
+
+  if (!_subMesh)
+    return decomposed;
+
+  auto vertexCount = _subMesh->VertexCount();
+  auto indexCount = _subMesh->IndexCount();
+  auto triangleCount = indexCount / 3u;
+
+  float *points = new float[vertexCount * 3u];
+  for (std::size_t i = 0; i < vertexCount; ++i)
+  {
+    std::size_t idx = i * 3u;
+    points[idx] = _subMesh->Vertex(i).X();
+    points[idx + 1] = _subMesh->Vertex(i).Y();
+    points[idx + 2] = _subMesh->Vertex(i).Z();
+  }
+
+  uint32_t *indices = new uint32_t[indexCount];
+  for (std::size_t i = 0; i < indexCount; ++i)
+  {
+    indices[i] = _subMesh->Index(i);
+  }
+
+  VHACD::IVHACD *iface = VHACD::CreateVHACD_ASYNC();
+  VHACD::IVHACD::Parameters parameters;
+  parameters.m_maxConvexHulls = _maxConvexHulls;
+  parameters.m_resolution = _voxelResolution;
+  parameters.m_asyncACD = true;
+  iface->Compute(points, vertexCount, indices, triangleCount, parameters);
+  while (!iface->IsReady())
+  {
+    std::this_thread::sleep_for(std::chrono::nanoseconds(10000));
+  }
+
+  if (!iface->GetNConvexHulls())
+    return decomposed;
+
+  for (std::size_t n = 0; n < iface->GetNConvexHulls(); ++n)
+  {
+    VHACD::IVHACD::ConvexHull ch;
+    iface->GetConvexHull(n, ch);
+
+    SubMesh convexMesh;
+    for (std::size_t i = 0u; i < ch.m_points.size(); ++i)
+    {
+      const VHACD::Vertex &p = ch.m_points[i];
+      gz::math::Vector3d vertex(p.mX, p.mY, p.mZ);
+      convexMesh.AddVertex(vertex);
+
+      // add dummy normal
+      auto norm = vertex;
+      norm.Normalize();
+      convexMesh.AddNormal(norm);
+    }
+
+    for (std::size_t i = 0u; i < ch.m_triangles.size(); ++i)
+    {
+      const VHACD::Triangle &tri = ch.m_triangles[i];
+      convexMesh.AddIndex(tri.mI0);
+      convexMesh.AddIndex(tri.mI1);
+      convexMesh.AddIndex(tri.mI2);
+    }
+    decomposed.push_back(convexMesh);
+  }
+
+  delete [] points;
+  delete [] indices;
+  iface->Release();
+
+  return decomposed;
+}

graphics/src/MeshManager_TEST.cc

@@ -23,6 +23,7 @@
 #include "gz/common/config.hh"
 
 #include "gz/common/testing/AutoLogFixture.hh"
+#include "gz/common/testing/TestPaths.hh"
 
 using namespace gz;
 
@@ -289,4 +290,45 @@ TEST_F(MeshManager, Remove)
   mgr->RemoveAll();
   EXPECT_FALSE(mgr->HasMesh("sphere"));
 }
+
+/////////////////////////////////////////////////
+TEST_F(MeshManager, ConvexDecomposition)
+{
+  auto mgr = common::MeshManager::Instance();
+  const common::Mesh *boxMesh = mgr->Load(
+      common::testing::TestFile("data", "box.dae"));
+
+  EXPECT_EQ(1u, boxMesh->SubMeshCount());
+  auto decomposed = mgr->ConvexDecomposition(
+      boxMesh->SubMeshByIndex(0u).lock().get());
+
+  // Decomposing a box should just produce a box
+  EXPECT_EQ(1u, decomposed.size());
+  common::SubMesh &boxSubmesh = decomposed[0];
+  // A convex hull of a box should contain exactly 8 vertices
+  EXPECT_EQ(8u, boxSubmesh.VertexCount());
+  EXPECT_EQ(8u, boxSubmesh.NormalCount());
+  EXPECT_EQ(36u, boxSubmesh.IndexCount());
+
+  const common::Mesh *drillMesh = mgr->Load(
+      common::testing::TestFile("data", "cordless_drill",
+      "meshes", "cordless_drill.dae"));
+  EXPECT_EQ(1u, drillMesh->SubMeshCount());
+  decomposed = mgr->ConvexDecomposition(
+      drillMesh->SubMeshByIndex(0u).lock().get());
+
+  // A drill should be decomposed into multiple submeshes
+  EXPECT_LT(1u, decomposed.size());
+  EXPECT_GE(32u, decomposed.size());
+  // Check submeshes are not empty
+  for (const auto &d : decomposed)
+  {
+    const common::SubMesh &drillSubmesh = d;
+    EXPECT_LT(3u, boxSubmesh.VertexCount());
+    EXPECT_EQ(drillSubmesh.VertexCount(), drillSubmesh.NormalCount());
+    EXPECT_LT(3u, drillSubmesh.IndexCount());
+  }
+}
+
+
 #endif