Add built-in hemisphere mesh.

PiperOrigin-RevId: 791148546
Change-Id: Ic9ff0c9b0f5fa4fd22d5f7f848f150cf74d378ba

doc/XMLreference.rst

@@ -1336,7 +1336,7 @@ The full list of processing steps applied by the compiler to each mesh is as fol
       **subdivision**: integer in [0-4]: The number of subdivisions to apply to icosahedron faces.
 
    :at-val:`hemisphere` (subdivision)
-      Repeated subdivisions :math:`s` of a square-based pyramid. Has :math:`1 + 2(s+1)(s+2)` vertices.
+      Repeated subdivisions :math:`s` of a square-based pyramid. Has :math:`2+2(s+1)(s+2)` vertices.
 
       **subdivision**: integer in [0-10]: The number of subdivisions to apply to the pyramid.
 

python/mujoco/specs.cc

@@ -1014,6 +1014,15 @@ PYBIND11_MODULE(_specs, m) {
       },
       py::arg("resolution") = std::array<int, 2>{0, 0},
       py::arg("fov") = std::array<double, 2>{0, 0}, py::arg("gamma") = 0);
+  mjsMesh.def(
+      "make_hemisphere",
+      [](raw::MjsMesh* self, int subdivision) {
+        double params[1] = {static_cast<double>(subdivision)};
+        if (mjs_makeMesh(self, mjMESH_BUILTIN_HEMISPHERE, params, 1)) {
+          throw pybind11::value_error(mjs_getError(mjs_getSpec(self->element)));
+        }
+      },
+      py::arg("subdivision"));
   mjsMesh.def(
       "make_cone",
       [](raw::MjsMesh* self, int nedge, double radius) {

python/mujoco/specs_test.py

@@ -496,11 +496,15 @@ class SpecsTest(absltest.TestCase):
     mesh = spec.add_mesh(name='cone')
     mesh.make_cone(nedge=6, radius=0)
 
+    mesh = spec.add_mesh(name='hemisphere')
+    mesh.make_hemisphere(subdivision=4)
+
     model = spec.compile()
-    self.assertEqual(model.nmesh, 3)
+    self.assertEqual(model.nmesh, 4)
     self.assertEqual(model.mesh_vertnum[0], 25 * 25)
     self.assertEqual(model.mesh_vertnum[1], 10)
     self.assertEqual(model.mesh_vertnum[2], 7)
+    self.assertEqual(model.mesh_vertnum[3], 2 * (4 + 1) * (4 + 2) + 2)
 
   def test_compile_errors_with_line_info(self):
     spec = mujoco.MjSpec()

src/user/user_api.cc

@@ -506,6 +506,24 @@ int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, int npar
   mjCMesh* meshC = static_cast<mjCMesh*>(mesh->element);
   mjCModel* m = meshC->model;
   switch (builtin) {
+    case mjMESH_BUILTIN_HEMISPHERE: {
+      if (nparams != 1) {
+        m->SetError(mjCError(0, "Hemisphere mesh type requires 1 parameter"));
+        return -1;
+      }
+      int subdiv = static_cast<int>(params[0]);
+      if (subdiv < 0) {
+        m->SetError(mjCError(0, "Hemisphere subdivision cannot be negative"));
+        return -1;
+      }
+      if (subdiv > 10) {
+        m->SetError(mjCError(0, "Hemisphere subdivision cannot be greater than 10"));
+        return -1;
+      }
+      meshC->MakeHemisphere(subdiv, /*make_faces*/ true, /*make_cap*/ true);
+      return 0;
+    }
+
     case mjMESH_BUILTIN_WEDGE: {
       if (nparams != 5) {
         m->SetError(mjCError(0, "Wedge builtin mesh types require 5 parameters"));

src/user/user_mesh.cc

@@ -2056,6 +2056,128 @@ void mjCMesh::CopyGraph() {
 
 
 
+// make a mesh of a hemisphere
+void mjCMesh::MakeHemisphere(int res, bool make_faces, bool make_cap) {
+  constexpr double kNorthPole[3] = {0, 0, 1};
+  constexpr double kEquator[4][3] = {
+      {1, 0, 0},
+      {0, 1, 0},
+      {-1, 0, 0},
+      {0, -1, 0},
+  };
+
+  // allocate vertices
+  int nvert = 1 + 2 * (res + 1) * (res + 2);
+  nvert += make_cap && make_faces;  // add center vertex for bottom cap faces
+  std::vector<float> vert(3 * nvert);
+
+  // north pole
+  vert[0] = kNorthPole[0];
+  vert[1] = kNorthPole[1];
+  vert[2] = kNorthPole[2];
+
+  // iterate through rows from north pole to equator, compute vertices
+  int v = 1;
+  for (int row = 0; row <= res; row++) {
+    // iterate through the four sides
+    for (int side = 0; side < 4; side++) {
+      double factor = static_cast<double>(row + 1) / (res + 1);
+      double start[3], end[3];
+
+      // start and end points of current arc
+      for (int i = 0; i < 3; i++) {
+        start[i] = kNorthPole[i] + factor * (kEquator[side][i] - kNorthPole[i]);
+        end[i] = kNorthPole[i] + factor * (kEquator[(side + 1) % 4][i] - kNorthPole[i]);
+      }
+
+      // step size for interpolation along the arc
+      double delta[3];
+      for (int i = 0; i < 3; i++) {
+        delta[i] = (end[i] - start[i]) / (row + 1);
+      }
+
+      // interpolate points along the arc
+      for (int i = 0; i < row + 1; i++) {
+        double p[3];
+        for (int j = 0; j < 3; j++) {
+          p[j] = start[j] + i * delta[j];
+        }
+
+        // normalize point to lie on hemisphere surface
+        double norm = std::sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]);
+        vert[3 * v + 0] = p[0] / norm;
+        vert[3 * v + 1] = p[1] / norm;
+        vert[3 * v + 2] = p[2] / norm;
+        v++;
+      }
+    }
+  }
+
+  // optional center vertex for bottom cap (for symmetry)
+  if (make_faces && make_cap) {
+    vert[3 * (nvert - 1) + 0] = 0;
+    vert[3 * (nvert - 1) + 1] = 0;
+    vert[3 * (nvert - 1) + 2] = 0;
+  }
+
+  // save vertices
+  mjs_setFloat(spec.uservert, vert.data(), 3 * nvert);
+
+  if (make_faces) {
+    // allocate faces
+    int nface = 4 * (res + 1) * (res + 1);
+    nface += make_cap * (4 * (res + 1));  // bottom cap faces
+    std::vector<int> face(3 * nface);
+
+    // faces connected to north pole
+    int f = 0;
+    face[f++] = 0; face[f++] = 1; face[f++] = 2;
+    face[f++] = 0; face[f++] = 2; face[f++] = 3;
+    face[f++] = 0; face[f++] = 3; face[f++] = 4;
+    face[f++] = 0; face[f++] = 4; face[f++] = 1;
+
+    // faces on the hemisphere from north pole to equator
+    for (int row = 0; row < res; row++) {
+      const int start_curr = 2 * row * (row + 1) + 1;
+      const int count_curr = 4 * (row + 1);
+      const int start_next = start_curr + count_curr;
+      const int count_next = 4 * (row + 2);
+
+      for (int side = 0; side < 4; side++) {
+        for (int i = 0; i < row + 2; i++) {
+          const int v_curr = i + (row + 1) * side;
+          const int v_next = i + (row + 2) * side;
+          face[f++] = start_curr + v_curr % count_curr;
+          face[f++] = start_next + v_next % count_next;
+          face[f++] = start_next + (v_next + 1) % count_next;
+
+          if (i < row + 1) {
+            face[f++] = start_curr + v_curr % count_curr;
+            face[f++] = start_next + (v_next + 1) % count_next;
+            face[f++] = start_curr + (v_curr + 1) % count_curr;
+          }
+        }
+      }
+    }
+
+    if (make_cap) {
+      // add faces for the bottom cap
+      const int start = 2 * res * (res + 1) + 1;
+      const int count = 4 * (res + 1);
+      for (int i = 0; i < count; i++) {
+        face[f++] = start + i;
+        face[f++] = nvert - 1;
+        face[f++] = start + (i + 1) % count;
+      }
+    }
+
+    // save faces
+    mjs_setInt(spec.userface, face.data(), 3 * nface);
+  }
+}
+
+
+
 // make a mesh of a spherical wedge
 void mjCMesh::MakeWedge(int resolution[2], double fov[2], double gamma) {
   std::vector<double> x_edges(resolution[0] + 1, 0);

src/user/user_objects.h

@@ -1051,6 +1051,7 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
   void NameSpace(const mjCModel* m);
 
   // make a mesh of a predefined shape
+  void MakeHemisphere(int res, bool make_faces, bool make_cap);
   void MakeWedge(int resolution[2], double fov[2], double gamma);
   void MakeRect(int resolution[2]);
   void MakeCone(int nedge, double radius);

test/user/testdata/makemesh.xml

@@ -1,5 +1,6 @@
 <mujoco model="makemesh">
   <asset>
+    <mesh name="hemisphere" builtin="hemisphere" params="4" scale=".8 .8 .8"/>
     <mesh name="wedge" builtin="wedge" params="10 10 60 60 0"/>
     <mesh name="prism3" builtin="cone" params="3 1" scale=".3 .3 .3"/>
     <mesh name="prism5" builtin="cone" params="5 1" scale=".3 .3 .1"/>
@@ -9,6 +10,7 @@
 
   <worldbody>
     <light pos="0 0 10"/>
+    <geom mesh="hemisphere" type="mesh" pos="-2 0 0"/>
     <geom mesh="wedge" type="mesh" euler="90 -90 0"/>
     <geom mesh="prism5" type="mesh" pos="2 0 0" euler="90 0 0"/>
     <geom mesh="prism3" type="mesh" pos="3 0 0"/>

test/user/user_mesh_test.cc

@@ -1288,6 +1288,27 @@ TEST_F(MjCMeshTest, OctreeNotComputedForNonSDF) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjCMeshTest, HemisphereSizes) {
+  static constexpr char xml[] = R"(
+  <mujoco model="makemesh">
+    <asset>
+      <mesh name="h0" builtin="hemisphere" params="0"/>
+      <mesh name="h1" builtin="hemisphere" params="1"/>
+      <mesh name="h2" builtin="hemisphere" params="2"/>
+    </asset>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << error.data();
+  EXPECT_EQ(model->mesh_vertnum[0], 2 * (0 + 1) * (0 + 2) + 2);
+  EXPECT_EQ(model->mesh_vertnum[1], 2 * (1 + 1) * (1 + 2) + 2);
+  EXPECT_EQ(model->mesh_vertnum[2], 2 * (2 + 1) * (2 + 2) + 2);
+  EXPECT_EQ(model->mesh_facenum[0], 4 * (0 + 1) * (0 + 2));
+  EXPECT_EQ(model->mesh_facenum[1], 4 * (1 + 1) * (1 + 2));
+  EXPECT_EQ(model->mesh_facenum[2], 4 * (2 + 1) * (2 + 2));
+  mj_deleteModel(model);
+}
 
 }  // namespace
 }  // namespace mujoco