Print warning and default to computing mesh inertia from convex hull if volume is negative (malformed mesh).

PiperOrigin-RevId: 526053615
Change-Id: I3974b3433da513f869b1bdab5e98232e215f6509

src/user/user_mesh.cc

@@ -877,6 +877,33 @@ void mjCMesh::LoadMSH(int default_provider) {
 }
 
 
+void mjCMesh::ComputeVolume(double CoM[3], mjtMeshType type,
+                              const double facecen[3], bool exactmeshinertia) {
+  double nrm[3];
+  double cen[3];
+  GetVolumeRef(type) = 0;
+  mjuu_zerovec(CoM, 3);
+  for (int i=0; i<nface; i++) {
+    // get area, normal and center
+    double a = _triangle(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
+
+    // compute and add volume
+    const double vec[3] = {cen[0]-facecen[0], cen[1]-facecen[1], cen[2]-facecen[2]};
+    double vol = type==mjSHELL_MESH ? a : mjuu_dot3(vec, nrm) * a / 3;
+
+    // if legacy computation requested, then always positive
+    if (!exactmeshinertia && type==mjVOLUME_MESH) {
+      vol = fabs(vol);
+    }
+
+    // add pyramid com
+    GetVolumeRef(type) += vol;
+    for (int j=0; j<3; j++) {
+      CoM[j] += vol*(cen[j]*3.0/4.0 + facecen[j]/4.0);
+    }
+  }
+}
+
 
 // apply transformations
 void mjCMesh::Process() {
@@ -888,6 +915,7 @@ void mjCMesh::Process() {
 
     double nrm[3];
     double cen[3];
+    bool exactmeshinertia = model->exactmeshinertia;
 
     if (type==mjVOLUME_MESH) {
       // translate
@@ -995,27 +1023,16 @@ void mjCMesh::Process() {
     }
 
     // compute CoM and volume from pyramid volumes
-    GetVolumeRef(type) = 0;
-    for (int i=0; i<nface; i++) {
-      // get area, normal and center
-      double a = _triangle(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
+    ComputeVolume(CoM, type, facecen, model->exactmeshinertia);
 
-      // compute and add volume
-      const double vec[3] = {cen[0]-facecen[0], cen[1]-facecen[1], cen[2]-facecen[2]};
-      double vol = type==mjSHELL_MESH ? a : mjuu_dot3(vec, nrm) * a / 3;
-
-      // if legacy computation requested, then always positive
-      if (!model->exactmeshinertia && type==mjVOLUME_MESH) {
-        vol = fabs(vol);
-      }
-
-      // add pyramid com
-      GetVolumeRef(type) += vol;
-      for (int j=0; j<3; j++) {
-        CoM[j] += vol*(cen[j]*3.0/4.0 + facecen[j]/4.0);
-      }
+    // perform computation again if volume is negative
+    if (GetVolumeRef(type) < mjMINVAL && exactmeshinertia) {
+      mju_warning("Malformed mesh %s, computing mesh inertia from convex hull", name.c_str());
+      exactmeshinertia = false;
+      ComputeVolume(CoM, type, facecen, exactmeshinertia);
     }
 
+
     // require positive volume
     if (GetVolumeRef(type) < mjMINVAL) {
       validvolume = GetVolumeRef(type) < 0 ? -1 : 0;
@@ -1051,7 +1068,7 @@ void mjCMesh::Process() {
       double vol = type==mjSHELL_MESH ? a : mjuu_dot3(cen, nrm) * a / 3;
 
       // if legacy computation requested, then always positive
-      if (!model->exactmeshinertia && type==mjVOLUME_MESH) {
+      if (!exactmeshinertia && type==mjVOLUME_MESH) {
         vol = fabs(vol);
       }
 

src/user/user_objects.h

@@ -523,6 +523,10 @@ class mjCMesh: public mjCBase {
   void RemoveRepeated(void);                  // remove repeated vertices
   void CheckMesh(void);                       // check if the mesh is valid
 
+  // compute the volume and center-of-mass of the mesh given the face center
+  void ComputeVolume(double CoM[3], mjtMeshType type, const double facecen[3],
+                     bool exactmeshinertia);
+
   // mesh properties that indicate a well-formed mesh
   std::pair<int, int> invalidorientation;     // indices of invalid edge; -1 if none
   bool validarea;                             // false if the area is too small

test/user/user_mesh_test.cc

@@ -464,7 +464,7 @@ TEST_F(MjCMeshTest, VolumeTooSmall) {
   EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small"));
 }
 
-TEST_F(MjCMeshTest, VolumeNegative) {
+TEST_F(MjCMeshTest, VolumeNegativeDefaultsLegacy) {
   static constexpr char xml[] = R"(
   <mujoco>
     <compiler exactmeshinertia="true"/>
@@ -482,8 +482,12 @@ TEST_F(MjCMeshTest, VolumeNegative) {
   )";
   std::array<char, 1024> error;
   mjModel* model = LoadModelFromString(xml, error.data(), error.size());
-  EXPECT_THAT(model, testing::IsNull());
-  EXPECT_THAT(error.data(), HasSubstr("mesh volume is negative"));
+  EXPECT_THAT(model, testing::NotNull());
+  EXPECT_LE(mju_abs(model->geom_size[0]), 1);
+  EXPECT_LE(mju_abs(model->geom_size[1]), 1);
+  EXPECT_LE(mju_abs(model->geom_size[2]), 1);
+  EXPECT_THAT(error.data(), HasSubstr("Malformed"));
+  mj_deleteModel(model);
 }
 
 TEST_F(MjCMeshTest, VolumeTooSmallAllowedWorld) {