From 996f0040fd08ac3db4ad81e6c764708fff5ad4ad Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 5 Aug 2025 05:04:11 -0700 Subject: [PATCH] Add built-in sphere mesh. PiperOrigin-RevId: 791162850 Change-Id: I573a414727e79f1a495e0f2282b2faff48ab3443 --- python/mujoco/specs.cc | 9 ++ python/mujoco/specs_test.py | 6 +- src/user/user_api.cc | 18 +++ src/user/user_mesh.cc | 105 ++++++++++++++++++ src/user/user_objects.h | 1 + .../collision_convex/perf/spheremesh.xml | 5 +- test/user/testdata/makemesh.xml | 4 +- test/user/user_mesh_test.cc | 25 ++++- 8 files changed, 165 insertions(+), 8 deletions(-) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 1e456578..b90d95ea 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_sphere", + [](raw::MjsMesh* self, int subdivision) { + double params[1] = {static_cast(subdivision)}; + if (mjs_makeMesh(self, mjMESH_BUILTIN_SPHERE, params, 1)) { + throw pybind11::value_error(mjs_getError(mjs_getSpec(self->element))); + } + }, + py::arg("subdivision")); mjsMesh.def( "make_hemisphere", [](raw::MjsMesh* self, int subdivision) { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index bf70e511..44617bc4 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -499,12 +499,16 @@ class SpecsTest(absltest.TestCase): mesh = spec.add_mesh(name='hemisphere') mesh.make_hemisphere(subdivision=4) + mesh = spec.add_mesh(name='sphere') + mesh.make_sphere(subdivision=2) + model = spec.compile() - self.assertEqual(model.nmesh, 4) + self.assertEqual(model.nmesh, 5) 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) + self.assertEqual(model.mesh_vertnum[4], 2 + 10 * 4**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 e513cb4f..1a8f7db1 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -524,6 +524,24 @@ int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, int npar return 0; } + case mjMESH_BUILTIN_SPHERE: { + if (nparams != 1) { + m->SetError(mjCError(0, "Sphere mesh type requires 1 parameter")); + return -1; + } + int subdiv = static_cast(params[0]); + if (subdiv < 0) { + m->SetError(mjCError(0, "Sphere subdivision cannot be negative")); + return -1; + } + if (subdiv > 4) { + m->SetError(mjCError(0, "Sphere subdivision cannot be greater than 4")); + return -1; + } + meshC->MakeSphere(subdiv, /*make_faces*/ 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 a4ed87e7..ab124c40 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -2178,6 +2179,110 @@ void mjCMesh::MakeHemisphere(int res, bool make_faces, bool make_cap) { +// make a mesh of a sphere using icosaheral subdivision +void mjCMesh::MakeSphere(int subdiv, bool make_faces) { + // make icosahedron + const float phi = (1.0 + std::sqrt(5.0)) / 2.0; + std::vector vert = { + -1.0, phi, 0.0, + 1.0, phi, 0.0, + -1.0, -phi, 0.0, + 1.0, -phi, 0.0, + + 0.0, -1.0, phi, + 0.0, 1.0, phi, + 0.0, -1.0, -phi, + 0.0, 1.0, -phi, + + phi, 0.0, -1.0, + phi, 0.0, 1.0, + -phi, 0.0, -1.0, + -phi, 0.0, 1.0, + }; + + // normalize vertices to be on a unit sphere + const double norm = std::sqrt(1.0 + phi * phi); + for (float& v : vert) { + v /= norm; + } + + std::vector face = { + 0, 11, 5, 0, 5, 1, 0, 1, 7, 0, 7, 10, 0, 10, 11, + 1, 5, 9, 5, 11, 4, 11, 10, 2, 10, 7, 6, 7, 1, 8, + 3, 9, 4, 3, 4, 2, 3, 2, 6, 3, 6, 8, 3, 8, 9, + 4, 9, 5, 2, 4, 11, 6, 2, 10, 8, 6, 7, 9, 8, 1 + }; + + // subdivision + if (subdiv > 0) { + // helper to get or create a midpoint vertex + auto get_midpoint = [&vert]( + int v1_idx, int v2_idx, std::map, int>& cache) -> int { + // key is the pair of vertex indices, sorted + std::pair key = std::minmax(v1_idx, v2_idx); + + // if midpoint is already in cache, return its index + auto it = cache.find(key); + if (it != cache.end()) { + return it->second; + } + + // otherwise, create it + const float* v1 = &vert[v1_idx * 3]; + const float* v2 = &vert[v2_idx * 3]; + + float mid_x = (v1[0] + v2[0]) / 2.0f; + float mid_y = (v1[1] + v2[1]) / 2.0f; + float mid_z = (v1[2] + v2[2]) / 2.0f; + + // normalize the new vertex to put it on the sphere + float mid_norm = std::sqrt(mid_x * mid_x + mid_y * mid_y + mid_z * mid_z); + mid_x /= mid_norm; + mid_y /= mid_norm; + mid_z /= mid_norm; + + // add the new vertex to the list + int new_idx = vert.size() / 3; + vert.push_back(mid_x); + vert.push_back(mid_y); + vert.push_back(mid_z); + + // add to cache + cache[key] = new_idx; + + return new_idx; + }; + + // subdivision loop + for (int i = 0; i < subdiv; ++i) { + std::map, int> midpoint_cache; + std::vector new_face; + new_face.reserve(face.size() * 4); + for (size_t j = 0; j < face.size(); j += 3) { + int v1 = face[j]; + int v2 = face[j+1]; + int v3 = face[j+2]; + + int m12 = get_midpoint(v1, v2, midpoint_cache); + int m23 = get_midpoint(v2, v3, midpoint_cache); + int m31 = get_midpoint(v3, v1, midpoint_cache); + + new_face.insert(new_face.end(), {v1, m12, m31}); + new_face.insert(new_face.end(), {v2, m23, m12}); + new_face.insert(new_face.end(), {v3, m31, m23}); + new_face.insert(new_face.end(), {m12, m23, m31}); + } + face = std::move(new_face); + } + } + + // save vertices and maybe faces + mjs_setFloat(spec.uservert, vert.data(), vert.size()); + if (make_faces) mjs_setInt(spec.userface, face.data(), face.size()); +} + + + // 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 7f26014b..cc00ea41 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1052,6 +1052,7 @@ class mjCMesh: public mjCMesh_, private mjsMesh { // make a mesh of a predefined shape void MakeHemisphere(int res, bool make_faces, bool make_cap); + void MakeSphere(int subdiv, bool make_faces); 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/engine/testdata/collision_convex/perf/spheremesh.xml b/test/engine/testdata/collision_convex/perf/spheremesh.xml index f6e747ad..995043aa 100644 --- a/test/engine/testdata/collision_convex/perf/spheremesh.xml +++ b/test/engine/testdata/collision_convex/perf/spheremesh.xml @@ -7,6 +7,7 @@ + @@ -38,8 +39,4 @@ - - - - diff --git a/test/user/testdata/makemesh.xml b/test/user/testdata/makemesh.xml index e6d0d1d8..19d61b4a 100644 --- a/test/user/testdata/makemesh.xml +++ b/test/user/testdata/makemesh.xml @@ -1,5 +1,6 @@ + @@ -10,7 +11,8 @@ - + + diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index b5190bb4..3cc8fbc4 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -15,6 +15,7 @@ // Tests for user/user_objects.cc. #include +#include #include #include #include @@ -732,7 +733,6 @@ TEST_F(MjCMeshTest, VolumeTooSmall) { EXPECT_THAT(model, testing::IsNull()); EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small")); mj_deleteModel(model); - } TEST_F(MjCMeshTest, VisualVolumeTooSmall) { @@ -760,7 +760,6 @@ TEST_F(MjCMeshTest, VisualVolumeTooSmall) { EXPECT_THAT(model, testing::IsNull()); EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small")); mj_deleteModel(model); - } TEST_F(MjCMeshTest, VisualVolumeSmallAllowedShell) { @@ -1310,5 +1309,27 @@ TEST_F(MjCMeshTest, HemisphereSizes) { mj_deleteModel(model); } +TEST_F(MjCMeshTest, SphereSizes) { + 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 + 10 * std::pow(4, 0)); + EXPECT_EQ(model->mesh_vertnum[1], 2 + 10 * std::pow(4, 1)); + EXPECT_EQ(model->mesh_vertnum[2], 2 + 10 * std::pow(4, 2)); + EXPECT_EQ(model->mesh_facenum[0], 20 * std::pow(4, 0)); + EXPECT_EQ(model->mesh_facenum[1], 20 * std::pow(4, 1)); + EXPECT_EQ(model->mesh_facenum[2], 20 * std::pow(4, 2)); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco