Add checks for degenerate meshes in convex hull computation.

Fixes #3260

PiperOrigin-RevId: 911610731
Change-Id: I9efd15eee1bb5cb13639fd733a49535a147c3401

src/user/user_mesh.cc

@@ -1715,6 +1715,78 @@ void mjCMesh::MakeGraph(const double* dvert) {
     return;
   }
 
+  // check for colocated/collinear/coplanar vertices
+  {
+    // find second vertex that is distinct from vertex 0
+    int v1 = -1;
+    double len1 = 0;
+    for (int i = 1; i < nvert(); i++) {
+      len1 = mjuu_dist3(dvert+3*i, dvert);
+      if (len1 > mjMINVAL) {
+        v1 = i;
+        break;
+      }
+    }
+
+    // no second vertex found: all vertices are colocated
+    if (v1 < 0) {
+      throw mjCError(this,
+          "mesh '%s' has colocated vertices, cannot compute convex hull."
+          " Consider using a small sphere instead",
+          name.c_str());
+    }
+
+    // find first non-collinear triple to define a plane
+    double edge1[3] = {dvert[3*v1+0] - dvert[0],
+                       dvert[3*v1+1] - dvert[1],
+                       dvert[3*v1+2] - dvert[2]};
+    double normal[3] = {0, 0, 0};
+    bool collinear = true;
+    for (int i = 1; i < nvert(); i++) {
+      if (i == v1) continue;
+      double edge2[3] = {dvert[3*i+0] - dvert[0],
+                         dvert[3*i+1] - dvert[1],
+                         dvert[3*i+2] - dvert[2]};
+      double len2 = sqrt(mjuu_dot3(edge2, edge2));
+      if (len2 < mjMINVAL) continue;
+      mjuu_crossvec(normal, edge1, edge2);
+      double norm = sqrt(mjuu_dot3(normal, normal));
+      if (norm > mjMINVAL * len1 * len2) {
+        normal[0] /= norm;
+        normal[1] /= norm;
+        normal[2] /= norm;
+        collinear = false;
+        break;
+      }
+    }
+
+    // vertices are collinear: cannot compute convex hull
+    if (collinear) {
+      throw mjCError(this,
+          "mesh '%s' has collinear vertices, cannot compute convex hull."
+          " Consider using a thin capsule instead",
+          name.c_str());
+    }
+
+    // find first vertex that is not on the plane
+    double d = mjuu_dot3(normal, dvert);
+    bool coplanar = true;
+    for (int i = 0; i < nvert(); i++) {
+      if (fabs(mjuu_dot3(normal, dvert+3*i) - d) > mjMINVAL * len1) {
+        coplanar = false;
+        break;
+      }
+    }
+
+    // vertices are coplanar: cannot compute convex hull
+    if (coplanar) {
+      throw mjCError(this,
+          "mesh '%s' has coplanar vertices, cannot compute convex hull."
+          " Consider using a primitive geom type (plane or thin box) instead",
+          name.c_str());
+    }
+  }
+
   qhT qh_qh;
   qhT* qh = &qh_qh;
   qh_zero(qh, stderr);

test/user/user_mesh_test.cc

@@ -713,7 +713,7 @@ TEST_F(MjCMeshTest, AreaTooSmall) {
   )";
   std::array<char, 1024> error;
   mjModel* model = LoadModelFromString(xml, error.data(), error.size());
-  EXPECT_THAT(model, testing::IsNull());
+  EXPECT_THAT(model, IsNull());
   EXPECT_THAT(error.data(), HasSubstr("mesh surface area is too small"));
 }
 
@@ -1385,6 +1385,63 @@ TEST_F(MjCMeshTest, QhullCache) {
   mj_deleteVFS(&vfs);
 }
 
+TEST_F(MjCMeshTest, ColocatedMeshError) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="example_mesh"
+            vertex="0 0 0  0 0 0  0 0 0  0 0 0"
+            face="0 1 2   1 3 2"/>
+    </asset>
+    <worldbody>
+      <geom type="mesh" mesh="example_mesh"/>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, IsNull());
+  EXPECT_THAT(error.data(), HasSubstr("colocated"));
+}
+
+TEST_F(MjCMeshTest, CollinearMeshError) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="example_mesh"
+            vertex="0 0 0  1 0 0  2 0 0  3 0 0"
+            face="0 1 2   1 3 2"/>
+    </asset>
+    <worldbody>
+      <geom type="mesh" mesh="example_mesh"/>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, IsNull());
+  EXPECT_THAT(error.data(), HasSubstr("collinear"));
+}
+
+TEST_F(MjCMeshTest, CoplanarMeshError) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="flat_quad"
+            vertex="-5 -5 0   5 -5 0   -5 5 0   5 5 0"
+            face="0 1 2   1 3 2"/>
+    </asset>
+    <worldbody>
+      <geom type="mesh" mesh="flat_quad"/>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, IsNull());
+  EXPECT_THAT(error.data(), HasSubstr("coplanar"));
+}
+
 TEST_F(MjCMeshTest, LoadSkin) {
   const std::string xml_path = GetTestDataFilePath(kCubeSkinPath);
   std::array<char, 1024> error;