From fcda45f7d793323ac1d177dc14dd3efbe6d3a434 Mon Sep 17 00:00:00 2001
From: Jasmeet Singh <jasmeet0915@gmail.com>
Date: Fri, 29 Sep 2023 22:40:09 +0530
Subject: [PATCH] Completed codecheck

Signed-off-by: Jasmeet Singh <jasmeet0915@gmail.com>
---
 src/MeshInertiaCalculator.cc | 60 +++++++++++++++++-------------------
 src/MeshInertiaCalculator.hh |  6 ++--
 2 files changed, 33 insertions(+), 33 deletions(-)

diff --git a/src/MeshInertiaCalculator.cc b/src/MeshInertiaCalculator.cc
index 27ceb64cf7..1350a9c257 100644
--- a/src/MeshInertiaCalculator.cc
+++ b/src/MeshInertiaCalculator.cc
@@ -109,9 +109,8 @@ void MeshInertiaCalculator::TransformInertiaMatrixToCOM(
   gz::math::Pose3d &_inertiaOrigin
 )
 {
-  std::cout << _centreOfMass << std::endl;
-  std::cout << _inertiaOrigin << std::endl;
-  gz::math::Vector3d comRelativeToOrigin = _centreOfMass.CoordPositionSub(_inertiaOrigin);
+  gz::math::Vector3d comRelativeToOrigin =
+    _centreOfMass.CoordPositionSub(_inertiaOrigin);
 
   gz::math::Vector3d ixxyyzz = _massMatrix.DiagonalMoments();
   gz::math::Vector3d ixyxzyz = _massMatrix.OffDiagonalMoments();
@@ -119,39 +118,40 @@ void MeshInertiaCalculator::TransformInertiaMatrixToCOM(
 
   // Transform the Inertia Matrix to COM using the
   // reverse of the Parallel Axis Theorem
-  ixxyyzz.X() = ixxyyzz.X() - mass * (comRelativeToOrigin.Y() * comRelativeToOrigin.Y()
-                                    + comRelativeToOrigin.Z() * comRelativeToOrigin.Z());
-  ixxyyzz.Y() = ixxyyzz.Y() - mass * (comRelativeToOrigin.X() * comRelativeToOrigin.X()
-                                    + comRelativeToOrigin.Z() * comRelativeToOrigin.Z());
-  ixxyyzz.Z() = ixxyyzz.Z() - mass * (comRelativeToOrigin.X() * comRelativeToOrigin.X()
-                                    + comRelativeToOrigin.Y() * comRelativeToOrigin.Y());
-
-  ixyxzyz.X() = ixyxzyz.X() + mass * comRelativeToOrigin.X() * comRelativeToOrigin.Y();
-  ixyxzyz.Y() = ixyxzyz.Y() + mass * comRelativeToOrigin.X() * comRelativeToOrigin.Z();
-  ixyxzyz.Z() = ixyxzyz.Z() + mass * comRelativeToOrigin.Y() * comRelativeToOrigin.Z();
+  ixxyyzz.X() = ixxyyzz.X() -
+                mass * (comRelativeToOrigin.Y() * comRelativeToOrigin.Y()
+                + comRelativeToOrigin.Z() * comRelativeToOrigin.Z());
+  ixxyyzz.Y() = ixxyyzz.Y() -
+                mass * (comRelativeToOrigin.X() * comRelativeToOrigin.X()
+                + comRelativeToOrigin.Z() * comRelativeToOrigin.Z());
+  ixxyyzz.Z() = ixxyyzz.Z() -
+                mass * (comRelativeToOrigin.X() * comRelativeToOrigin.X()
+                + comRelativeToOrigin.Y() * comRelativeToOrigin.Y());
+
+  ixyxzyz.X() = ixyxzyz.X() +
+                mass * comRelativeToOrigin.X() * comRelativeToOrigin.Y();
+  ixyxzyz.Y() = ixyxzyz.Y() +
+                mass * comRelativeToOrigin.X() * comRelativeToOrigin.Z();
+  ixyxzyz.Z() = ixyxzyz.Z() +
+                mass * comRelativeToOrigin.Y() * comRelativeToOrigin.Z();
 
   _massMatrix.SetDiagonalMoments(ixxyyzz);
   _massMatrix.SetOffDiagonalMoments(ixyxzyz);
 
   gz::math::Quaterniond rotOffset = _massMatrix.PrincipalAxesOffset();
-  
-  std::cout << "Before Rotation: " << std::endl;
-  std::cout << _massMatrix.Moi() << std::endl;
 
+  // If there is a rotational offset remove that
   if (rotOffset != gz::math::Quaterniond::Identity)
   {
     gz::math::Matrix3d rotMat = gz::math::Matrix3d(rotOffset);
-    gz::math::Matrix3d rotatedMoi = rotMat.Transposed() * _massMatrix.Moi() * rotMat;
+    gz::math::Matrix3d rotatedMoi =
+      rotMat.Transposed() * _massMatrix.Moi() * rotMat;
     _massMatrix.SetMoi(rotatedMoi);
-    
+
     // This is required since the rotation will place the
-    // diagonal elements in an ascending order which might not
-    // be the original one
+    // principal moments on the diagonal in an ascending order
+    // which might not be the original one
     _massMatrix.SetDiagonalMoments(ixxyyzz);
-
-    std::cout << _massMatrix.PrincipalMoments() << std::endl;
-    std::cout << _massMatrix.DiagonalMoments() << std::endl;
-    std::cout << _massMatrix.Moi() << std::endl;
   }
 }
 
@@ -315,15 +315,13 @@ std::optional<gz::math::Inertiald> MeshInertiaCalculator::operator()
   this->CalculateMassProperties(meshTriangles, density,
     meshMassMatrix, inertiaOrigin);
 
+  // The if the mesh origin (about which the inertia matrix was calculated)
+  // is not the mesh centroid (center of mass), then transform the inertia
+  // matrix to the COM
   if (inertiaOrigin != centreOfMass)
   {
-    // TODO(jasmeet0915): tranform the calculated inertia matrix
-    // from inertia origin to centre of mass
-    gzwarn << "Calculated centroid does not match the mesh origin. "
-    "Inertia Tensor values won't be correct. Use mesh with origin at "
-    "geometric center." << std::endl;
-
-    this->TransformInertiaMatrixToCOM(meshMassMatrix, centreOfMass, inertiaOrigin);
+    this->TransformInertiaMatrixToCOM(
+      meshMassMatrix, centreOfMass, inertiaOrigin);
   }
 
   gz::math::Inertiald meshInertial;
diff --git a/src/MeshInertiaCalculator.hh b/src/MeshInertiaCalculator.hh
index b20d1bb270..a706afd07d 100644
--- a/src/MeshInertiaCalculator.hh
+++ b/src/MeshInertiaCalculator.hh
@@ -114,8 +114,10 @@ namespace gz
         /// to which the inertia matrix has to be transformed.
         /// \param[in] _inertiaOrigin The point about which the inertia matrix
         /// was calculated. This would be the origin of the mesh.
-        public: void TransformInertiaMatrixToCOM(gz::math::MassMatrix3d &_massMatrix,
-          gz::math::Pose3d &_centreOfMass, gz::math::Pose3d &_inertiaOrigin);
+        public: void TransformInertiaMatrixToCOM(
+          gz::math::MassMatrix3d &_massMatrix,
+          gz::math::Pose3d &_centreOfMass,
+          gz::math::Pose3d &_inertiaOrigin);
 
         /// \brief Function that calculates the mass, mass matrix & centre of
         /// mass of a mesh using a vector of Triangles of the mesh