Ensured correct mass is set during MoI matrix generation from bounding object #4906 (#6048)

* ensured correct mass is set during I matrix generation from bounding box

* ensured correct mass is set during I matrix generation from bounding box

* code refactoring #4906

* Update changelog-r2023.md

logging fix for bug #4906

* Update src/webots/nodes/WbSolid.cpp

Co-authored-by: Stefania Pedrazzi <[email protected]>

* removing unused constant

* refactoring of reference density in WbSolid.cpp

* adjusted changes for clang format

* Update docs/reference/changelog-r2023.md

Co-authored-by: Stefania Pedrazzi <[email protected]>

---------

Co-authored-by: Olivier Michel <[email protected]>
Co-authored-by: Stefania Pedrazzi <[email protected]>

Author: 3 people

docs/reference/changelog-r2023.md

@@ -54,6 +54,7 @@ Released on ??
     - Fixed unwanted altitude change when reaching a target waypoint in `mavic2pro_patrol.c` ([#5981](https://github.com/cyberbotics/webots/pull/5981)).
     - Fixed the extern controller connection to a target Webots instance when a snap one is running ([#6002](https://github.com/cyberbotics/webots/pull/6002)).
     - Fixed the double downloading of meshes ([#6034](https://github.com/cyberbotics/webots/pull/6034)).
+    - Fixed generation of moment of inertia matrix based on bounding box ([#6048](https://github.com/cyberbotics/webots/pull/6048)).
     - Fixed the loading of remote mesh bounding objects ([#6047](https://github.com/cyberbotics/webots/pull/6047)).
 
 ## Webots R2023a

src/webots/nodes/WbSolid.cpp

@@ -78,6 +78,7 @@ using namespace WbSolidUtilities;
 using namespace std;
 
 const double WbSolid::MASS_ZERO_THRESHOLD = 1e-10;
+const double REFERENCE_DENSITY = 1000.0;
 
 QList<const WbSolid *> WbSolid::cSolids;
 
@@ -979,7 +980,7 @@ void WbSolid::adjustOdeMass(bool mergeMass) {
     } else {
       const double fieldDensity = p->density();
       if (fieldDensity >= 0.0)
-        dMassAdjust(mOdeMass, (currentMass * fieldDensity) / 1000.0);
+        dMassAdjust(mOdeMass, (currentMass * fieldDensity) / REFERENCE_DENSITY);
     }
 
     memcpy(mMassAroundCoM, mOdeMass, sizeof(dMass));
@@ -1404,7 +1405,7 @@ void WbSolid::setInertiaMatrixFromBoundingObject() {
   dMassSetZero(&dmass);
 
   // Adds the masses of all the primitives lying in the bounding object
-  WbSolidUtilities::addMass(&dmass, boundingObject(), 1000.0);
+  WbSolidUtilities::addMass(&dmass, boundingObject(), REFERENCE_DENSITY);
   memcpy(mReferenceMass, &dmass, sizeof(dMass));
 
   // Computes the inertia matrix around the center of mass of the bounding object
@@ -1414,18 +1415,25 @@ void WbSolid::setInertiaMatrixFromBoundingObject() {
   const WbField *const parameter = findField("physics", true)->parameter();
   WbPhysics *const p = parameter ? static_cast<WbPhysics *>(static_cast<WbSFNode *>(parameter->value())->value()) : physics();
 
-  const double s = 1.0 / absoluteScale().x();
+  const double s0 = absoluteScale().x();
+  const double s = 1.0 / s0;
   double s3 = s * s;
   double s5 = s3;
   s3 *= s;
 
-  if (p->mass() < 0.0) {
-    double boundingObjectMass = mReferenceMass->mass;
-    boundingObjectMass *= 0.001 * p->density();
+  // Sets the actual total mass to mReferenceMass
+  double boundingObjectMass = mReferenceMass->mass;
+  if (p->mass() > 0.0)
+    boundingObjectMass = s0 * s0 * s0 * p->mass();
+  else {
+    boundingObjectMass *= p->density() / REFERENCE_DENSITY;
     p->setMass(boundingObjectMass * s3, true);
     p->parsingInfo(tr("'mass' set as bounding object's mass based on 'density'."));
   }
 
+  // Adjust the total according to mass and density fields
+  dMassAdjust(mReferenceMass, boundingObjectMass);
+
   p->setDensity(-1.0, true);
 
   const double *const I = mReferenceMass->I;
@@ -1806,7 +1814,7 @@ void WbSolid::createOdeMass(bool reset) {
   const WbPhysics *const p = physics();
   const bool customMass = p->mode() == WbPhysics::CUSTOM_INERTIA_MATRIX;
   // needed for average density and average damping
-  WbSolidUtilities::addMass(mReferenceMass, boundingObject(), 1000.0, !customMass);
+  WbSolidUtilities::addMass(mReferenceMass, boundingObject(), REFERENCE_DENSITY, !customMass);
 
   // Checks whether there is a valid inertia matrix, and uses it if so
   if (customMass)
@@ -1835,8 +1843,8 @@ void WbSolid::createOdeMass(bool reset) {
     if (fieldMass > 0.0) {
       const double s2 = s * s;
       actualMass = s * s2 * fieldMass;
-    } else if (fieldDensity != 1000.0)
-      actualMass *= 0.001 * fieldDensity;
+    } else if (fieldDensity != REFERENCE_DENSITY)
+      actualMass *= fieldDensity / REFERENCE_DENSITY;
 
     // Adjust the total according to mass and density fields
     dMassAdjust(mOdeMass, actualMass);