Adds new function in MeshManager for performing convex decomposition (#…

…585)


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
+      /// are 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

@@ -16,11 +16,27 @@
  */
 
 #include <sys/stat.h>
-#include <string>
+
+#include <cctype>
+#include <cstdint>
 #include <mutex>
+#include <string>
 #include <unordered_map>
 #include <unordered_set>
-#include <cctype>
+#include <vector>
+
+// Suppress warnings for VHACD
+#ifndef _WIN32
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wfloat-equal"
+#pragma GCC diagnostic ignored "-Wswitch-default"
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#endif
+#define ENABLE_VHACD_IMPLEMENTATION 1
+#include "VHACD/VHACD.h"
+#ifndef _WIN32
+#pragma GCC diagnostic pop
+#endif
 
 #ifndef _WIN32
   #include "gz/common/GTSMeshUtils.hh"
@@ -35,6 +51,7 @@
 #include "gz/common/ColladaExporter.hh"
 #include "gz/common/OBJLoader.hh"
 #include "gz/common/STLLoader.hh"
+#include "gz/common/Timer.hh"
 #include "gz/common/Util.hh"
 #include "gz/common/config.hh"
 
@@ -1649,3 +1666,97 @@ 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;
+
+  auto vertexCount = _subMesh.VertexCount();
+  auto indexCount = _subMesh.IndexCount();
+  auto triangleCount = indexCount / 3u;
+
+  std::vector<float> points;
+  points.resize(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();
+  }
+
+  std::vector<uint32_t> indices;
+  indices.resize(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.data(), vertexCount, indices.data(), triangleCount,
+                 parameters);
+
+  common::Timer t;
+  t.Start();
+  auto timeout = std::chrono::seconds(300);
+  while (!iface->IsReady())
+  {
+    if (t.ElapsedTime() > timeout)
+    {
+      iface->Cancel();
+      gzwarn << "Convex decomposition timed out. Process took more than "
+             << timeout.count() << " seconds. "  << std::endl;
+      t.Stop();
+      break;
+    }
+    std::this_thread::sleep_for(std::chrono::nanoseconds(10000));
+  }
+
+  if (!iface->GetNConvexHulls())
+  {
+    gzwarn << "No convex hulls are generated "
+           << (!_subMesh.Name().empty() ? "from " : "")
+           <<  _subMesh.Name();
+    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 - this will be overriden by the
+      // RecalculateNormals call
+      convexMesh.AddNormal(vertex);
+    }
+
+    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);
+    }
+    convexMesh.RecalculateNormals();
+    decomposed.push_back(convexMesh);
+  }
+
+  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,51 @@ 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"));
+
+  ASSERT_NE(nullptr, boxMesh);
+  EXPECT_EQ(1u, boxMesh->SubMeshCount());
+
+  std::size_t maxConvexHulls = 4;
+  std::size_t resolution = 1000;
+  auto submesh = boxMesh->SubMeshByIndex(0u).lock();
+  auto decomposed = std::move(mgr->ConvexDecomposition(
+      *(submesh.get()), maxConvexHulls, resolution));
+
+  // 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"));
+  ASSERT_NE(nullptr, drillMesh);
+  EXPECT_EQ(1u, drillMesh->SubMeshCount());
+  submesh = drillMesh->SubMeshByIndex(0u).lock();
+  decomposed = std::move(mgr->ConvexDecomposition(
+      *(submesh.get()), maxConvexHulls, resolution));
+
+  // A drill should be decomposed into multiple submeshes
+  EXPECT_LT(1u, decomposed.size());
+  EXPECT_GE(maxConvexHulls, decomposed.size());
+  // Check submeshes are not empty
+  for (const auto &d : decomposed)
+  {
+    const common::SubMesh &drillSubmesh = d;
+    EXPECT_LT(3u, drillSubmesh.VertexCount());
+    EXPECT_EQ(drillSubmesh.VertexCount(), drillSubmesh.NormalCount());
+    EXPECT_LT(3u, drillSubmesh.IndexCount());
+  }
+}
+
 #endif

graphics/src/VHACD/LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2011, Khaled Mamou (kmamou at gmail dot com)
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

graphics/src/VHACD/README.md

@@ -0,0 +1,3 @@
+V-HACD: https://github.com/kmammou/v-hacd
+commit: ab01cba71cc563b73bd3f0d574d346b2b797ea4b
+