Merge pull request #6087 from cyberbotics/sync-master-faddc2077

Merge master into develop

Author: Benjamin Délèze

docs/reference/changelog-r2023.md

@@ -40,6 +40,7 @@ Released on ??
       - `Field.enableSFTracking` and `Field.disableSFTracking` which were failing ([#5640](https://github.com/cyberbotics/webots/pull/5640)).
       - `Motor.enableForceFeedback` where the `sampling_period` argument was missing ([#5797](https://github.com/cyberbotics/webots/pull/5797)).
       - `Motor.setAcceleration` was missing the device tag parameter ([#6036](https://github.com/cyberbotics/webots/pull/6036)).
+      - `Lidar.getRangeImageArray` had a typo in its name ([#6080](https://github.com/cyberbotics/webots/issues/6080)).
     - Fixed crash resulting from requesting pose tracking of unsuitable nodes ([#5620](https://github.com/cyberbotics/webots/pull/5620)).
     - Fixed memory leaks, particularly when in no-rendering mode and spawning/deleting nodes ([#5639](https://github.com/cyberbotics/webots/pull/5639)).
     - Fixed crashes resulting from streaming pose, SF field values or contact points after deleting the tracked nodes ([#5638](https://github.com/cyberbotics/webots/pull/5638)).
@@ -69,6 +70,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

lib/controller/python/controller/lidar.py

@@ -73,7 +73,7 @@ def getNumberOfPoints(self) -> int:
     def getRangeImage(self) -> List[float]:
         return self.range_image[:self.horizontal_resolution * self.number_of_layers]
 
-    def defRangeImageArray(self) -> List[List[float]]:
+    def getRangeImageArray(self) -> List[List[float]]:
         array = []
         for i in range(self.number_of_layers):
             array.append(self.getLayerRangeImage(i))

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;
 
@@ -989,7 +990,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));
@@ -1414,7 +1415,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
@@ -1424,18 +1425,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;
@@ -1816,7 +1824,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)
@@ -1845,8 +1853,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);