From 5220767457f08586386aea7c941d75b9b776b5c7 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 5 Aug 2025 04:15:27 -0700 Subject: [PATCH] Add built-in hemisphere mesh. PiperOrigin-RevId: 791148546 Change-Id: Ic9ff0c9b0f5fa4fd22d5f7f848f150cf74d378ba --- doc/XMLreference.rst | 2 +- python/mujoco/specs.cc | 9 +++ python/mujoco/specs_test.py | 6 +- src/user/user_api.cc | 18 +++++ src/user/user_mesh.cc | 122 ++++++++++++++++++++++++++++++++ src/user/user_objects.h | 1 + test/user/testdata/makemesh.xml | 2 + test/user/user_mesh_test.cc | 21 ++++++ 8 files changed, 179 insertions(+), 2 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 5500d7cd..e3805a69 100644 --- a/doc/XMLreference.rst +++ b/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. diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 9953d212..1e456578 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -1014,6 +1014,15 @@ PYBIND11_MODULE(_specs, m) { }, py::arg("resolution") = std::array{0, 0}, py::arg("fov") = std::array{0, 0}, py::arg("gamma") = 0); + mjsMesh.def( + "make_hemisphere", + [](raw::MjsMesh* self, int subdivision) { + double params[1] = {static_cast(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) { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index e1c728a1..bf70e511 100644 --- a/python/mujoco/specs_test.py +++ b/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() diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 92ee8030..e513cb4f 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -506,6 +506,24 @@ int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, int npar mjCMesh* meshC = static_cast(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(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")); diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index c33024b3..a4ed87e7 100644 --- a/src/user/user_mesh.cc +++ b/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 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(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 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 x_edges(resolution[0] + 1, 0); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index f2fc5661..7f26014b 100644 --- a/src/user/user_objects.h +++ b/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); diff --git a/test/user/testdata/makemesh.xml b/test/user/testdata/makemesh.xml index 91955db9..e6d0d1d8 100644 --- a/test/user/testdata/makemesh.xml +++ b/test/user/testdata/makemesh.xml @@ -1,5 +1,6 @@ + @@ -9,6 +10,7 @@ + diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index dfa736bf..b5190bb4 100644 --- a/test/user/user_mesh_test.cc +++ b/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"( + + + + + + + + )"; + std::array 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