Add mesh optimization attribute to <mesh> (#1382)

Signed-off-by: Ian Chen <ichen@openrobotics.org>
Signed-off-by: Steve Peters <scpeters@openrobotics.org>
Co-authored-by: Steve Peters <scpeters@openrobotics.org>

include/sdf/Mesh.hh

@@ -37,6 +37,47 @@ namespace sdf
   // Forward declarations.
   class ParserConfig;
 
+  /// \brief Mesh optimization method
+  enum class MeshOptimization
+  {
+    /// \brief No mesh optimization
+    NONE,
+    /// \brief Convex hull
+    CONVEX_HULL,
+    /// \brief Convex decomposition
+    CONVEX_DECOMPOSITION
+  };
+
+  /// \brief Convex decomposition
+  class SDFORMAT_VISIBLE ConvexDecomposition
+  {
+    /// \brief Default constructor
+    public: ConvexDecomposition();
+
+    /// \brief Load the contact based on a element pointer. This is *not* the
+    /// usual entry point. Typical usage of the SDF DOM is through the Root
+    /// object.
+    /// \param[in] _sdf The SDF Element pointer
+    /// \return Errors, which is a vector of Error objects. Each Error includes
+    /// an error code and message. An empty vector indicates no error.
+    public: Errors Load(ElementPtr _sdf);
+
+    /// \brief Get a pointer to the SDF element that was used during
+    /// load.
+    /// \return SDF element pointer. The value will be nullptr if Load has
+    /// not been called.
+    public: sdf::ElementPtr Element() const;
+
+    /// \brief Get the maximum number of convex hulls that can be generated.
+    public: unsigned int MaxConvexHulls() const;
+
+    /// \brief Set the maximum number of convex hulls that can be generated.
+    public: void SetMaxConvexHulls(unsigned int _maxConvexHulls);
+
+    /// \brief Private data pointer.
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+
   /// \brief Mesh represents a mesh shape, and is usually accessed through a
   /// Geometry.
   class SDFORMAT_VISIBLE Mesh
@@ -61,6 +102,37 @@ namespace sdf
     /// an error code and message. An empty vector indicates no error.
     public: Errors Load(sdf::ElementPtr _sdf, const ParserConfig &_config);
 
+    /// \brief Get the mesh's optimization method
+    /// \return The mesh optimization method.
+    /// MeshOptimization::NONE if no mesh simplificaton is done.
+    public: MeshOptimization Optimization() const;
+
+    /// \brief Get the mesh's optimization method
+    /// \return The mesh optimization method.
+    /// Empty string if no mesh simplificaton is done.
+    public: std::string OptimizationStr() const;
+
+    /// \brief Set the mesh optimization method.
+    /// \param[in] _optimization The mesh optimization method.
+    public: void SetOptimization(MeshOptimization _optimization);
+
+    /// \brief Set the mesh optimization method.
+    /// \param[in] _optimization The mesh optimization method.
+    /// \return True if the _optimizationStr parameter matched a known
+    /// mesh optimization method. False if the mesh optimization method
+    /// could not be set.
+    public: bool SetOptimization(const std::string &_optimizationStr);
+
+    /// \brief Get the associated ConvexDecomposition object
+    /// \returns Pointer to the associated ConvexDecomposition object,
+    /// nullptr if the Mesh doesn't contain a ConvexDecomposition element.
+    public: const sdf::ConvexDecomposition *ConvexDecomposition() const;
+
+    /// \brief Set the associated ConvexDecomposition object.
+    /// \param[in] _convexDecomposition The ConvexDecomposition object.
+    public: void SetConvexDecomposition(
+        const sdf::ConvexDecomposition &_convexDecomposition);
+
     /// \brief Get the mesh's URI.
     /// \return The URI of the mesh data.
     public: std::string Uri() const;

sdf/1.11/mesh_shape.sdf

@@ -1,5 +1,19 @@
 <element name="mesh" required="0">
   <description>Mesh shape</description>
+
+  <attribute name="optimization" type="string" default="" required="0">
+    <description>
+      Set whether to optimize the mesh using one of the specified methods. Values include: "convex_hull" - a single convex hull that encapsulates the mesh, "convex_decomposition" - decompose the mesh into multiple convex hull meshes. Default value is an empty string which means no mesh optimization.
+    </description>
+  </attribute>
+
+  <element name="convex_decomposition" required="0">
+    <description>Convex decomposition parameters. Applicable if the mesh optimization attribute is set to convex_decomposition</description>
+    <element name="max_convex_hulls" type="unsigned int" default="16" required="0">
+      <description>Maximum number of convex hulls to decompose into. If the input mesh has multiple submeshes, this limit is applied when decomposing each submesh</description>
+    </element>
+  </element>
+
   <element name="uri" type="string" default="__default__" required="1">
     <description>Mesh uri</description>
   </element>

sdf/1.12/mesh_shape.sdf

@@ -1,5 +1,19 @@
 <element name="mesh" required="0">
   <description>Mesh shape</description>
+
+  <attribute name="optimization" type="string" default="" required="0">
+    <description>
+      Set whether to optimize the mesh using one of the specified methods. Values include: "convex_hull" - a single convex hull that encapsulates the mesh, "convex_decomposition" - decompose the mesh into multiple convex hull meshes. Default value is an empty string which means no mesh optimization.
+    </description>
+  </attribute>
+
+  <element name="convex_decomposition" required="0">
+    <description>Convex decomposition parameters. Applicable if the mesh optimization attribute is set to convex_decomposition</description>
+    <element name="max_convex_hulls" type="unsigned int" default="16" required="0">
+      <description>Maximum number of convex hulls to decompose into. If the input mesh has multiple submeshes, this limit is applied when decomposing each submesh</description>
+    </element>
+  </element>
+
   <element name="uri" type="string" default="__default__" required="1">
     <description>Mesh uri</description>
   </element>

src/Mesh.cc

@@ -14,8 +14,10 @@
  * limitations under the License.
  *
 */
+#include <array>
 #include <filesystem>
 #include <optional>
+#include <string_view>
 
 #include <gz/math/Inertial.hh>
 #include "sdf/CustomInertiaCalcProperties.hh"
@@ -27,9 +29,35 @@
 
 using namespace sdf;
 
-// Private data class
+/// Mesh Optimization method strings. These should match the data in
+/// `enum class MeshOptimization` located in Mesh.hh, and the size
+/// template parameter should match the number of elements as well.
+constexpr std::array<const std::string_view, 3> kMeshOptimizationStrs =
+{
+  "",
+  "convex_hull",
+  "convex_decomposition"
+};
+
+// Private data class for ConvexDecomposition
+class sdf::ConvexDecomposition::Implementation
+{
+  /// \brief Maximum number of convex hulls to generate.
+  public: unsigned int maxConvexHulls{16u};
+
+  /// \brief The SDF element pointer used during load.
+  public: sdf::ElementPtr sdf = nullptr;
+};
+
+// Private data class for Mesh
 class sdf::Mesh::Implementation
 {
+  /// \brief Mesh optimization method
+  public: MeshOptimization optimization = MeshOptimization::NONE;
+
+  /// \brief Optional convex decomposition.
+  public: std::optional<sdf::ConvexDecomposition> convexDecomposition;
+
   /// \brief The mesh's URI.
   public: std::string uri = "";
 
@@ -49,6 +77,62 @@ class sdf::Mesh::Implementation
   public: sdf::ElementPtr sdf = nullptr;
 };
 
+/////////////////////////////////////////////////
+ConvexDecomposition::ConvexDecomposition()
+  : dataPtr(gz::utils::MakeImpl<Implementation>())
+{
+}
+
+/////////////////////////////////////////////////
+Errors ConvexDecomposition::Load(ElementPtr _sdf)
+{
+  Errors errors;
+
+  this->dataPtr->sdf = _sdf;
+
+  // Check that sdf is a valid pointer
+  if (!_sdf)
+  {
+    errors.push_back({ErrorCode::ELEMENT_MISSING,
+        "Attempting to load convex decomposition, "
+        "but the provided SDF element is null."});
+    return errors;
+  }
+
+  // We need a convex_decomposition element
+  if (_sdf->GetName() != "convex_decomposition")
+  {
+    errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE,
+        "Attempting to load convex decomposition, but the provided SDF "
+        "element is not <convex_decomposition>."});
+    return errors;
+  }
+
+  this->dataPtr->maxConvexHulls = _sdf->Get<unsigned int>(
+      errors, "max_convex_hulls",
+      this->dataPtr->maxConvexHulls).first;
+
+  return errors;
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr ConvexDecomposition::Element() const
+{
+  return this->dataPtr->sdf;
+}
+
+/////////////////////////////////////////////////
+unsigned int ConvexDecomposition::MaxConvexHulls() const
+{
+  return this->dataPtr->maxConvexHulls;
+}
+
+/////////////////////////////////////////////////
+void ConvexDecomposition::SetMaxConvexHulls(unsigned int _maxConvexHulls)
+{
+  this->dataPtr->maxConvexHulls = _maxConvexHulls;
+}
+
 /////////////////////////////////////////////////
 Mesh::Mesh()
   : dataPtr(gz::utils::MakeImpl<Implementation>())
@@ -61,6 +145,7 @@ Errors Mesh::Load(ElementPtr _sdf)
   return this->Load(_sdf, ParserConfig::GlobalConfig());
 }
 
+
 /////////////////////////////////////////////////
 Errors Mesh::Load(ElementPtr _sdf, const ParserConfig &_config)
 {
@@ -87,6 +172,20 @@ Errors Mesh::Load(ElementPtr _sdf, const ParserConfig &_config)
     return errors;
   }
 
+  // Optimization
+  if (_sdf->HasAttribute("optimization"))
+  {
+    this->SetOptimization(_sdf->Get<std::string>("optimization", "").first);
+  }
+
+  if (_sdf->HasElement("convex_decomposition"))
+  {
+    this->dataPtr->convexDecomposition.emplace();
+    Errors err = this->dataPtr->convexDecomposition->Load(
+        _sdf->GetElement("convex_decomposition", errors));
+    errors.insert(errors.end(), err.begin(), err.end());
+  }
+
   if (_sdf->HasElement("uri"))
   {
     std::unordered_set<std::string> paths;
@@ -140,6 +239,56 @@ sdf::ElementPtr Mesh::Element() const
   return this->dataPtr->sdf;
 }
 
+//////////////////////////////////////////////////
+MeshOptimization Mesh::Optimization() const
+{
+  return this->dataPtr->optimization;
+}
+
+//////////////////////////////////////////////////
+std::string Mesh::OptimizationStr() const
+{
+  size_t index = static_cast<int>(this->dataPtr->optimization);
+  if (index < kMeshOptimizationStrs.size())
+    return std::string(kMeshOptimizationStrs[index]);
+  return "";
+}
+
+//////////////////////////////////////////////////
+bool Mesh::SetOptimization(const std::string &_optimizationStr)
+{
+  for (size_t i = 0; i < kMeshOptimizationStrs.size(); ++i)
+  {
+    if (_optimizationStr == kMeshOptimizationStrs[i])
+    {
+      this->dataPtr->optimization = static_cast<MeshOptimization>(i);
+      return true;
+    }
+  }
+  return false;
+}
+
+//////////////////////////////////////////////////
+void Mesh::SetOptimization(MeshOptimization _optimization)
+{
+  this->dataPtr->optimization = _optimization;
+}
+
+//////////////////////////////////////////////////
+const sdf::ConvexDecomposition *Mesh::ConvexDecomposition() const
+{
+  if (this->dataPtr->convexDecomposition.has_value())
+    return &this->dataPtr->convexDecomposition.value();
+  return nullptr;
+}
+
+//////////////////////////////////////////////////
+ void Mesh::SetConvexDecomposition(
+    const sdf::ConvexDecomposition &_convexDecomposition)
+{
+  this->dataPtr->convexDecomposition = _convexDecomposition;
+}
+
 //////////////////////////////////////////////////
 std::string Mesh::Uri() const
 {
@@ -244,6 +393,18 @@ sdf::ElementPtr Mesh::ToElement(sdf::Errors &_errors) const
   sdf::ElementPtr elem(new sdf::Element);
   sdf::initFile("mesh_shape.sdf", elem);
 
+  // Optimization
+  elem->GetAttribute("optimization")->Set<std::string>(
+      this->OptimizationStr());
+
+  if (this->dataPtr->convexDecomposition.has_value())
+  {
+    sdf::ElementPtr convexDecomp = elem->GetElement("convex_decomposition",
+        _errors);
+    convexDecomp->GetElement("max_convex_hulls")->Set(
+        this->dataPtr->convexDecomposition->MaxConvexHulls());
+  }
+
   // Uri
   sdf::ElementPtr uriElem = elem->GetElement("uri", _errors);
   uriElem->Set(_errors, this->Uri());