From 445d3bd64fcb50e021e17382cfb27bbc5421b934 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Wed, 27 Sep 2023 06:09:31 -0700
Subject: [PATCH] Collision: add optional density parameter to API

Add std::optional<double> parameter to override the
density in Collision::CalculateInertial. It is not
yet used, but will hold the new API before release.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 include/sdf/Collision.hh | 13 +++++++++----
 src/Collision.cc         |  3 ++-
 2 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh
index d6b8470c9..7770f6042 100644
--- a/include/sdf/Collision.hh
+++ b/include/sdf/Collision.hh
@@ -18,6 +18,7 @@
 #define SDF_COLLISION_HH_
 
 #include <memory>
+#include <optional>
 #include <string>
 #include <gz/math/Pose3.hh>
 #include <gz/math/Vector3.hh>
@@ -145,12 +146,16 @@ namespace sdf
     /// \brief Calculate and return the MassMatrix for the collision
     /// \param[out] _errors A vector of Errors objects. Each errors contains an
     /// Error code and a message. An empty errors vector indicates no errors
-    /// \param[in] _config Custom parser configuration
     /// \param[out] _inertial An inertial object which will be set with the
     /// calculated inertial values
-    public: void CalculateInertial(sdf::Errors &_errors,
-                                  gz::math::Inertiald &_inertial,
-                                  const ParserConfig &_config);
+    /// \param[in] _config Custom parser configuration
+    /// \param[in] _density An optional density value to override this
+    /// collision's density.
+    public: void CalculateInertial(
+                    sdf::Errors &_errors,
+                    gz::math::Inertiald &_inertial,
+                    const ParserConfig &_config,
+                    const std::optional<double> &_density = std::nullopt);
 
     /// \brief Get a pointer to the SDF element that was used during
     /// load.
diff --git a/src/Collision.cc b/src/Collision.cc
index c2b2a15ec..9f01b6fd0 100644
--- a/src/Collision.cc
+++ b/src/Collision.cc
@@ -254,7 +254,8 @@ sdf::SemanticPose Collision::SemanticPose() const
 void Collision::CalculateInertial(
   sdf::Errors &_errors,
   gz::math::Inertiald &_inertial,
-  const ParserConfig &_config)
+  const ParserConfig &_config,
+  const std::optional<double> &/*_density*/)
 {
   // Check if density was not set during load & send a warning
   // about the default value being used