Process meshes for computing the correct scaling even when they have flipped faces.

PiperOrigin-RevId: 469414367
Change-Id: Ia5c75b4f2c9fd592c81a1ec659720f9820c0dfbb

src/user/user_mesh.cc

@@ -86,6 +86,7 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
   validvolume = true;
   valideigenvalue = true;
   validinequality = true;
+  processed = false;
 
   // reset to default if given
   if (_def) {
@@ -254,9 +255,8 @@ void mjCMesh::Compile(const mjVFS* vfs) {
   }
 
   // scale, center, orient, compute mass and inertia
-  if (validorientation) {
+  Process();
-    Process();
+  processed = true;
-  }
 }
 
 
@@ -1165,6 +1165,9 @@ void mjCMesh::Process() {
 
 // check that the mesh is valid
 void mjCMesh::CheckMesh() {
+  if (!processed) {
+    return;
+  }
   if (!validorientation)
     throw mjCError(this, "faces have inconsistent orientation: %s", name.c_str());
   if (!validarea)
@@ -1178,24 +1181,16 @@ void mjCMesh::CheckMesh() {
 }
 
 
-// compute inertia
+// get inertia pointer
 double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) {
   CheckMesh();
-  if (type==mjSHELL_MESH) {
+  return type==mjSHELL_MESH ? boxsz_surface : boxsz_volume;
-    return boxsz_surface;
-  } else {
-    return boxsz_volume;
-  }
 }
 
 
 double& mjCMesh::GetVolumeRef(mjtMeshType type) {
   CheckMesh();
-  if (type) {
+  return type==mjSHELL_MESH ? surface : volume;
-    return surface;
-  } else {
-    return volume;
-  }
 }
 
 

src/user/user_objects.h

@@ -491,6 +491,7 @@ class mjCMesh: public mjCBase {
   bool validvolume;                   // false if the volume is too small
   bool valideigenvalue;               // false if inertia eigenvalue is too small
   bool validinequality;               // false if inertia inequality is not satisfied
+  bool processed;                     // false if the mesh has not been processed yet
 
   // mesh properties computed by Compile
   double pos_volume[3];               // CoM position

test/user/user_mesh_test.cc

@@ -213,6 +213,16 @@ TEST_F(MujocoTest, FlippedFaceFails) {
   EXPECT_THAT(error.data(), HasSubstr("faces have inconsistent orientation"));
 }
 
+void CheckTetrahedronWasRescaled(mjModel* model) {
+  // the rotated and rescaled positions of the standard tetrahedron
+  mjtNum vert[] = {
+    -mju_sqrt(3)/4, 0., 0, mju_sqrt(3)/12, 0, mju_sqrt(6)/3, mju_sqrt(3)/12,
+    -mju_sqrt(2)/2, -mju_sqrt(6)/6, mju_sqrt(3)/12, mju_sqrt(2)/2, -mju_sqrt(6)/6};
+  for (int i=0; i<12; ++i) {
+    EXPECT_NEAR(model->mesh_vert[i], vert[i], std::numeric_limits<float>::epsilon());
+  }
+}
+
 TEST_F(MujocoTest, FlippedFaceAllowedWorld) {
   static constexpr char xml[] = R"(
   <mujoco>
@@ -229,6 +239,7 @@ TEST_F(MujocoTest, FlippedFaceAllowedWorld) {
   std::array<char, 1024> error;
   mjModel* model = LoadModelFromString(xml, error.data(), error.size());
   EXPECT_THAT(model, testing::NotNull());
+  CheckTetrahedronWasRescaled(model);
   mj_deleteModel(model);
 }
 
@@ -250,6 +261,7 @@ TEST_F(MujocoTest, FlippedFaceAllowedNoMass) {
   std::array<char, 1024> error;
   mjModel* model = LoadModelFromString(xml, error.data(), error.size());
   EXPECT_THAT(model, testing::NotNull());
+  CheckTetrahedronWasRescaled(model);
   mj_deleteModel(model);
 }
 
@@ -272,6 +284,7 @@ TEST_F(MujocoTest, FlippedFaceAllowedInertial) {
   std::array<char, 1024> error;
   mjModel* model = LoadModelFromString(xml, error.data(), error.size());
   EXPECT_THAT(model, testing::NotNull());
+  CheckTetrahedronWasRescaled(model);
   mj_deleteModel(model);
 }