From cab173f755c29d0395e02090de0257aa12af8fc3 Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@openrobotics.org>
Date: Fri, 15 Mar 2024 23:12:16 +0000
Subject: [PATCH] simplification -> optimization

Signed-off-by: Ian Chen <ichen@openrobotics.org>
---
 bullet-featherstone/src/Base.cc        |  7 ++++++
 bullet-featherstone/src/SDFFeatures.cc | 33 +++++++++++---------------
 2 files changed, 21 insertions(+), 19 deletions(-)

diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc
index 13dd6c8fa..a64aced52 100644
--- a/bullet-featherstone/src/Base.cc
+++ b/bullet-featherstone/src/Base.cc
@@ -42,6 +42,13 @@ WorldInfo::WorldInfo(std::string name_)
   // Needed for force-torque sensor
   this->world->getSolverInfo().m_jointFeedbackInJointFrame = true;
   this->world->getSolverInfo().m_jointFeedbackInWorldSpace = false;
+
+  // By default a large impulse is applied when mesh collisions penetrate
+  // which causes unstable behavior. Bullet featherstone does not support
+  // configuring split impulse and penetration threshold parameters. Instead
+  // the penentration impulse depends on the erp2 parameter so set to a small
+  // value (default is 0.2).
+  this->world->getSolverInfo().m_erp2 = btScalar(0.02);
 }
 
 }  // namespace bullet_featherstone
diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc
index d20f205d4..6d18b57b5 100644
--- a/bullet-featherstone/src/SDFFeatures.cc
+++ b/bullet-featherstone/src/SDFFeatures.cc
@@ -1034,14 +1034,6 @@ bool SDFFeatures::AddSdfCollision(
   }
   else if (const auto *meshSdf = geom->MeshShape())
   {
-    // By default a large impulse is applied when mesh collisions penetrate
-    // which causes unstable behavior. Bullet featherstone does not support
-    // configuring split impulse and penetration threshold parameters. Instead
-    // the penentration impulse depends on the erp2 parameter so set to a small
-    // value (default is 0.2).
-    auto *world = this->ReferenceInterface<WorldInfo>(model->world);
-    world->world->getSolverInfo().m_erp2 = btScalar(0.02);
-
     auto &meshManager = *gz::common::MeshManager::Instance();
     auto *mesh = meshManager.Load(meshSdf->Uri());
     const btVector3 scale = convertVec(meshSdf->Scale());
@@ -1060,23 +1052,26 @@ bool SDFFeatures::AddSdfCollision(
     {
       auto s = mesh->SubMeshByIndex(submeshIdx).lock();
       bool meshCreated = false;
-      if (meshSdf->Simplification() ==
-          ::sdf::MeshSimplification::CONVEX_DECOMPOSITION ||
-          meshSdf->Simplification() ==
-          ::sdf::MeshSimplification::CONVEX_HULL)
+      if (meshSdf->Optimization() ==
+          ::sdf::MeshOptimization::CONVEX_DECOMPOSITION ||
+          meshSdf->Optimization() ==
+          ::sdf::MeshOptimization::CONVEX_HULL)
       {
-        std::vector<common::SubMesh> decomposed;
-        if (meshSdf->Simplification() == ::sdf::MeshSimplification::CONVEX_HULL)
+        std::size_t maxConvexHulls = 16u;
+        if (meshSdf->Optimization() == ::sdf::MeshOptimization::CONVEX_HULL)
         {
           /// create 1 convex hull for the whole submesh
-          decomposed = std::move(meshManager.ConvexDecomposition(*s.get(), 1u));
+          maxConvexHulls = 1u;
         }
-        else
+        else if (meshSdf->ConvexDecomposition())
         {
-          /// decompose into multiple convex hulls
-          decomposed = std::move(meshManager.ConvexDecomposition(*s.get()));
+          // limit max number of convex hulls to generate
+          maxConvexHulls = meshSdf->ConvexDecomposition()->MaxConvexHulls();
         }
-        gzdbg << "Simplifying mesh using convex decomposition. " << std::endl;
+        std::vector<common::SubMesh> decomposed
+            = std::move(meshManager.ConvexDecomposition(*s.get(), maxConvexHulls));
+
+        gzdbg << "Optimizing mesh using convex decomposition. " << std::endl;
         if (!s->Name().empty())
           gzdbg << "  Submesh: " << s->Name() << std::endl;
         gzdbg << "  Mesh: " << mesh->Name() << std::endl;