Do not raise an error for flipped triangles if their area contribution is negligible.

PiperOrigin-RevId: 474552367
Change-Id: I9727a74e50a9bab9b04bb0de0b64e11587ed36f1

src/user/user_mesh.cc

@@ -48,6 +48,38 @@ extern "C" {
 using std::string;
 using std::vector;
 
+// compute triangle area, surface normal, center
+static mjtNum _triangle(mjtNum* normal, mjtNum* center,
+                        const float* v1, const float* v2, const float* v3) {
+  // center
+  if (center) {
+    for (int i=0; i<3; i++) {
+      center[i] = (v1[i] + v2[i] + v3[i])/3;
+    }
+  }
+
+  // normal = (v2-v1) cross (v3-v1)
+  double b[3] = { v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2] };
+  double c[3] = { v3[0]-v1[0], v3[1]-v1[1], v3[2]-v1[2] };
+  mju_cross(normal, b, c);
+
+  // get length
+  double len = mju_norm3(normal);
+
+  // ignore small faces
+  if (len<mjMINVAL) {
+    return 0;
+  }
+
+  // normalize
+  normal[0] /= len;
+  normal[1] /= len;
+  normal[2] /= len;
+
+  // return area
+  return len/2;
+}
+
 //------------------ class mjCMesh implementation --------------------------------------------------
 
 // constructor
@@ -212,15 +244,27 @@ void mjCMesh::Compile(const mjVFS* vfs) {
     face = (int*) mju_malloc(3*nface*sizeof(int));
     memcpy(face, userface.data(), 3*nface*sizeof(int));
 
+    // check vertices exist
+    for (auto vertex_index : userface) {
+      if (vertex_index>=nvert || vertex_index < 0) {
+        throw mjCError(this, "found index in userface that exceeds uservert size.");
+      }
+    }
+
     // create half-edge structure (if mesh was in XML)
     if (useredge.empty()) {
       for (int i=0; i<nface; i++) {
         int v0 = userface[3*i+0];
         int v1 = userface[3*i+1];
         int v2 = userface[3*i+2];
-        useredge.push_back(std::pair(v0, v1));
+        mjtNum normal[3];
-        useredge.push_back(std::pair(v1, v2));
+        if (_triangle(normal, nullptr, vert+3*v0, vert+3*v1, vert+3*v2)>sqrt(mjMINVAL)) {
-        useredge.push_back(std::pair(v2, v0));
+          useredge.push_back(std::pair(v0, v1));
+          useredge.push_back(std::pair(v1, v2));
+          useredge.push_back(std::pair(v2, v0));
+        } else {
+          mju_warning("Degenerate triangle found, its orientation will not be checked.");
+        }
       }
     }
   }
@@ -554,6 +598,7 @@ void mjCMesh::LoadOBJ(const mjVFS* vfs) {
     bool has_normals = !attrib.normals.empty();
     bool has_texcoords = !attrib.texcoords.empty();
     bool righthand = (scale[0]*scale[1]*scale[2] > 0);
+
     // iterate over mesh faces
     int index_in_mesh_indices = 0;
     for (int face = 0; face < mesh.num_face_vertices.size(); face++) {
@@ -562,6 +607,7 @@ void mjCMesh::LoadOBJ(const mjVFS* vfs) {
             this, "only tri or quad meshes are supported for OBJ (file '%s')",
             filename.c_str());
       }
+
       // add face
       std::vector<std::array<tinyobj::index_t, 3>> faces;
       tinyobj::index_t v0 = mesh.indices[index_in_mesh_indices];
@@ -569,18 +615,12 @@ void mjCMesh::LoadOBJ(const mjVFS* vfs) {
       tinyobj::index_t v2 = mesh.indices[index_in_mesh_indices+2];
       std::array<tinyobj::index_t, 3> face1 = {v0, v1, v2};
       faces.push_back(face1);
-      // add edges
+
-      useredge.push_back(std::pair(v0.vertex_index, v1.vertex_index));
-      useredge.push_back(std::pair(v1.vertex_index, v2.vertex_index));
-      useredge.push_back(std::pair(v2.vertex_index, v0.vertex_index));
       // handle quad: add second triangle with 4th vertex
       if (mesh.num_face_vertices[face] == 4) {
         tinyobj::index_t v3 = mesh.indices[index_in_mesh_indices+3];
         std::array<tinyobj::index_t, 3> face2 = {v0, v2, v3};
         faces.push_back(face2);
-        useredge.push_back(std::pair(v0.vertex_index, v2.vertex_index));
-        useredge.push_back(std::pair(v2.vertex_index, v3.vertex_index));
-        useredge.push_back(std::pair(v3.vertex_index, v0.vertex_index));
       }
       for (const auto& face_indices : faces) {
         int index_of_first_vertex = uservert.size()/3;
@@ -590,6 +630,7 @@ void mjCMesh::LoadOBJ(const mjVFS* vfs) {
               uservert.end(),
               attrib.vertices.begin() + 3*tinyobj_index.vertex_index,
               attrib.vertices.begin() + 3*tinyobj_index.vertex_index + 3);
+
           // for each vertex, add its normal
           if (has_normals) {
             usernormal.insert(
@@ -597,6 +638,7 @@ void mjCMesh::LoadOBJ(const mjVFS* vfs) {
                 attrib.normals.begin() + 3*tinyobj_index.normal_index,
                 attrib.normals.begin() + 3*tinyobj_index.normal_index + 3);
           }
+
           // for each vertex, add two entries to usertexcoord
           if (has_texcoords) {
             usertexcoord.push_back(
@@ -605,14 +647,33 @@ void mjCMesh::LoadOBJ(const mjVFS* vfs) {
                 1-attrib.texcoords[2*tinyobj_index.texcoord_index + 1]);
           }
         }
-        // add vertex indices (in uservert) to userface
+        int i0 = index_of_first_vertex;
-        userface.push_back(index_of_first_vertex);
+        int i1 = index_of_first_vertex+1;
-        if (righthand) {
+        int i2 = index_of_first_vertex+2;
-          userface.push_back(index_of_first_vertex+1);
+
-          userface.push_back(index_of_first_vertex+2);
+        // add edges
+        const float *v0 = uservert.data() + 3*i0;
+        const float *v1 = uservert.data() + 3*i1;
+        const float *v2 = uservert.data() + 3*i2;
+        mjtNum normal[3];
+
+        // only consider edges if the face contribution is significant
+        if (_triangle(normal, nullptr, v0, v1, v2)>sqrt(mjMINVAL)) {
+          useredge.push_back(std::pair(face_indices[0].vertex_index, face_indices[1].vertex_index));
+          useredge.push_back(std::pair(face_indices[1].vertex_index, face_indices[2].vertex_index));
+          useredge.push_back(std::pair(face_indices[2].vertex_index, face_indices[0].vertex_index));
         } else {
-          userface.push_back(index_of_first_vertex+2);
+          mju_warning("Degenerate triangle found, its orientation will not be checked.");
-          userface.push_back(index_of_first_vertex+1);
+        }
+
+        // add vertex indices (in uservert) to userface
+        userface.push_back(i0);
+        if (righthand) {
+          userface.push_back(i1);
+          userface.push_back(i2);
+        } else {
+          userface.push_back(i2);
+          userface.push_back(i1);
         }
       }
       index_in_mesh_indices += mesh.num_face_vertices[face];
@@ -859,41 +920,6 @@ void mjCMesh::LoadMSH(const mjVFS* vfs) {
 
 
 
-// compute triangle area, surface normal, center
-static double _areaNrmCen(double* normal, double* center,
-                          const float* v1, const float* v2, const float* v3) {
-  // center
-  if (center) {
-    for (int i=0; i<3; i++) {
-      center[i] = (v1[i] + v2[i] + v3[i])/3;
-    }
-  }
-
-  // normal = (v2-v1) cross (v3-v1)
-  double b[3] = { v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2] };
-  double c[3] = { v3[0]-v1[0], v3[1]-v1[1], v3[2]-v1[2] };
-  normal[0] = b[1]*c[2] - b[2]*c[1];
-  normal[1] = b[2]*c[0] - b[0]*c[2];
-  normal[2] = b[0]*c[1] - b[1]*c[0];
-
-  // get length
-  double len = sqrt(normal[0]*normal[0] + normal[1]*normal[1] + normal[2]*normal[2]);
-
-  // ignore small faces
-  if (len<mjMINVAL) {
-    return 0;
-  }
-
-  // normalize
-  normal[0] /= len;
-  normal[1] /= len;
-  normal[2] /= len;
-
-  // return area
-  return len/2;
-}
-
-
 // apply transformations
 void mjCMesh::Process() {
   for ( const auto type : { mjtMeshType::mjVOLUME_MESH, mjtMeshType::mjSHELL_MESH } ) {
@@ -992,7 +1018,7 @@ void mjCMesh::Process() {
         }
 
         // get area and center
-        double a = _areaNrmCen(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
+        double a = _triangle(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
 
         // accumulate
         for (j=0; j<3; j++) {
@@ -1017,7 +1043,7 @@ void mjCMesh::Process() {
     GetVolumeRef(type) = 0;
     for (i=0; i<nface; i++) {
       // get area, normal and center
-      double a = _areaNrmCen(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
+      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]};
@@ -1066,7 +1092,7 @@ void mjCMesh::Process() {
       float* F = vert+3*face[3*i+2];
 
       // get area, normal and center; update volume
-      double a = _areaNrmCen(nrm, cen, D, E, F);
+      double a = _triangle(nrm, cen, D, E, F);
       double vol = type==mjSHELL_MESH ? a : mjuu_dot3(cen, nrm) * a / 3;
 
       // if legacy computation requested, then always positive

test/user/user_mesh_test.cc

@@ -288,6 +288,28 @@ TEST_F(MujocoTest, FlippedFaceAllowedInertial) {
   mj_deleteModel(model);
 }
 
+TEST_F(MujocoTest, FlippedFaceAllowedNegligibleArea) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="example_mesh"
+        vertex="0 0 0  1 0 0  0 1 0  0 0 1  0 0 1"
+        face="2 0 3  0 1 3  1 2 3  0 2 1  0 3 4" />
+    </asset>
+    <worldbody>
+      <body>
+        <geom type="mesh" mesh="example_mesh"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, testing::NotNull());
+  CheckTetrahedronWasRescaled(model);
+  mj_deleteModel(model);
+}
+
 TEST_F(MujocoTest, AreaTooSmall) {
   static constexpr char xml[] = R"(
   <mujoco>