Link.cc changes

gistfile1.txt

diff --git a/src/Link.cc b/src/Link.cc
index c74783a1..7c5a342b 100644
--- a/src/Link.cc
+++ b/src/Link.cc
@@ -80,6 +80,9 @@ class sdf::Link::Implementation
 
   /// \brief Scoped Pose Relative-To graph at the parent model scope.
   public: sdf::ScopedGraph<sdf::PoseRelativeToGraph> poseRelativeToGraph;
+
+  public: bool autoInertia = false;
+  public: bool autoInertiaSaved = false;
 };
 
 /////////////////////////////////////////////////
@@ -111,6 +114,7 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config)
     return errors;
   }
 
+  this->dataPtr->auto_inertia = _sdf->Get<bool>("auto_inertia");
   // Read the links's name
   if (!loadName(_sdf, this->dataPtr->name))
   {
@@ -838,6 +842,13 @@ sdf::ElementPtr Link::ToElement() const
   inertiaElem->GetElement("iyy")->Set(massMatrix.Iyy());
   inertiaElem->GetElement("iyz")->Set(massMatrix.Iyz());
   inertiaElem->GetElement("izz")->Set(massMatrix.Izz());
+  if (!this->dataPtr->autoInertiaSaved)
+  {
+    if (this->dataPtr->autoInertia)
+    {
+      inertialElem->GetElement("auto_inertia")->Set(this->dataPtr->autoInertia);
+    }
+  }
 
   if (this->dataPtr->inertial.FluidAddedMass().has_value())
   {
@@ -907,3 +918,25 @@ sdf::ElementPtr Link::ToElement() const
 
   return elem;
 }
+bool Link::AutoInertia()
+Link::SetAutoInertia(bool value)
+
+  enum ClacConfig
+{
+  SAVE_CALCULATION,
+  NO_SAVE_CALCULATION,
+  RESET_CALCULATION
+}
+
+CalculateInertia(CalcConfig _param)
+{
+  if (!this->AutoInertia())
+    return
+
+  if (_param == RESET_CALCULATION || !this->dataPtr->autoInertiaSaved){
+      massMatrix = ...;
+  }
+  // Do the actual calculation
+  if (_param == SAVE_CALCULATION)
+    this->dataPtr->autoInertiaSaved = true;
+}