Cone: set inertial center of mass Z = -L/4

Signed-off-by: Steve Peters <[email protected]>
rudislabs · May 23, 2024 · 8f77c6a · 8f77c6a
1 parent ec0f10f
commit 8f77c6a
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 1 deletion.
diff --git a/src/Cone.cc b/src/Cone.cc
@@ -159,6 +159,7 @@ std::optional<gz::math::Inertiald> Cone::CalculateInertial(double _density)
   {
     gz::math::Inertiald coneInertial;
     coneInertial.SetMassMatrix(coneMassMatrix.value());
+    coneInertial.SetPose({0, 0, -0.25 * this->dataPtr->cone.Length(), 0, 0, 0});
     return std::make_optional(coneInertial);
   }
 }

diff --git a/src/Cone_TEST.cc b/src/Cone_TEST.cc
@@ -219,7 +219,7 @@ TEST(DOMCone, CalculateInertial)
 
   gz::math::Inertiald expectedInertial;
   expectedInertial.SetMassMatrix(expectedMassMat);
-  expectedInertial.SetPose(gz::math::Pose3d::Zero);
+  expectedInertial.SetPose({0, 0, -l / 4.0, 0, 0, 0});
 
   auto coneInertial = cone.CalculateInertial(density);
   EXPECT_EQ(cone.Shape().Mat().Density(), density);