From 5ef84a9a793496558aeb43303d90e790654539e6 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 28 Sep 2023 04:58:56 +0530 Subject: [PATCH] Update Mesh Inertia Calculator to incorporate mesh scale in the calculations (#2171) Signed-off-by: Jasmeet Singh --- src/MeshInertiaCalculator.cc | 11 +++++++++-- src/MeshInertiaCalculator.hh | 5 ++++- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/MeshInertiaCalculator.cc b/src/MeshInertiaCalculator.cc index e80f2cfd91..060d0afc43 100644 --- a/src/MeshInertiaCalculator.cc +++ b/src/MeshInertiaCalculator.cc @@ -39,7 +39,8 @@ using namespace sim; ////////////////////////////////////////////////// void MeshInertiaCalculator::GetMeshTriangles( - std::vector& _triangles, + std::vector &_triangles, + const gz::math::Vector3d &_meshScale, const gz::common::Mesh* _mesh) { // Get the vertices & indices of the mesh @@ -60,6 +61,12 @@ void MeshInertiaCalculator::GetMeshTriangles( triangle.v2.X() = vertArray[static_cast(3 * indArray[i+2])]; triangle.v2.Y() = vertArray[static_cast(3 * indArray[i+2] + 1)]; triangle.v2.Z() = vertArray[static_cast(3 * indArray[i+2] + 2)]; + + // Apply mesh scale to the triangle coordinates + triangle.v0 = triangle.v0 * _meshScale; + triangle.v1 = triangle.v1 * _meshScale; + triangle.v2 = triangle.v2 * _meshScale; + triangle.centroid = (triangle.v0 + triangle.v1 + triangle.v2) / 3; _triangles.push_back(triangle); } @@ -245,7 +252,7 @@ std::optional MeshInertiaCalculator::operator() gz::math::Pose3d inertiaOrigin; // Create a list of Triangle objects from the mesh vertices and indices - this->GetMeshTriangles(meshTriangles, mesh); + this->GetMeshTriangles(meshTriangles, sdfMesh->Scale(), mesh); // Calculate the mesh centroid and set is as centre of mass this->CalculateMeshCentroid(centreOfMass, meshTriangles); diff --git a/src/MeshInertiaCalculator.hh b/src/MeshInertiaCalculator.hh index 62687d8ef8..8d865fcd2d 100644 --- a/src/MeshInertiaCalculator.hh +++ b/src/MeshInertiaCalculator.hh @@ -82,9 +82,12 @@ namespace gz /// \param[out] _triangles A vector to hold all the Triangle /// instances obtained /// from the vertices & indices of the mesh + /// \param[in] _meshScale A vector with the scaling factor + /// of all the 3 axes /// \param[in] _mesh Mesh object public: void GetMeshTriangles( - std::vector& _triangles, + std::vector &_triangles, + const gz::math::Vector3d &_meshScale, const gz::common::Mesh* _mesh); /// \brief Function to calculate the centroid of the mesh. Since