diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index e3805a69..87879764 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1348,11 +1348,15 @@ The full list of processing steps applied by the compiler to each mesh is as fol **nvert**: integer >= 3: The number vertices in the polygon. |br| **radius**: real in [0, 1]: The radius of the top face. - :at-val:`torus` (radius, resolution) - A torus with major radius of 1 and given minor radius. + :at-val:`supertorus` (resolution, radius, s, t) + A generalization of a torus with major radius of 1 and given minor radius. If the **s** and **t** parameters are + both 1, the shape is a torus. See `here `__ for details regarding + the definition of supertori. - **radius**: real in (0, 1]: The minor radius of the torus. - |br| **resolution** integer >= 4: The discretization of both major and minor radii. + **resolution** integer >= 4: The discretization of both major and minor radii. + |br| **radius**: real in (0, 1]: The minor radius of the torus. + |br| **s**: real > 0: The "squareness" of major sections. + |br| **t**: real > 0: The "squareness" of minor sections. :at-val:`wedge` (res_phi, res_theta, fov_phi, fov_theta, gamma) A slice of a unit spherical shell in spherical coordinates. diff --git a/doc/includes/references.h b/doc/includes/references.h index 4b7e2e70..6cb4f586 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1789,7 +1789,7 @@ typedef enum mjtMeshBuiltin_ { // type of built-in procedural mesh mjMESH_BUILTIN_SPHERE, // sphere mjMESH_BUILTIN_HEMISPHERE, // hemisphere mjMESH_BUILTIN_CONE, // cone - mjMESH_BUILTIN_TORUS, // torus + mjMESH_BUILTIN_SUPERTORUS, // supertorus mjMESH_BUILTIN_WEDGE, // wedge mjMESH_BUILTIN_PLATE, // plate } mjtMeshBuiltin; diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index e0c856cf..d0a9f2d2 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -75,7 +75,7 @@ typedef enum mjtMeshBuiltin_ { // type of built-in procedural mesh mjMESH_BUILTIN_SPHERE, // sphere mjMESH_BUILTIN_HEMISPHERE, // hemisphere mjMESH_BUILTIN_CONE, // cone - mjMESH_BUILTIN_TORUS, // torus + mjMESH_BUILTIN_SUPERTORUS, // supertorus mjMESH_BUILTIN_WEDGE, // wedge mjMESH_BUILTIN_PLATE, // plate } mjtMeshBuiltin; diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index 72905fb6..00ce1084 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -801,7 +801,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjMESH_BUILTIN_SPHERE', 1), ('mjMESH_BUILTIN_HEMISPHERE', 2), ('mjMESH_BUILTIN_CONE', 3), - ('mjMESH_BUILTIN_TORUS', 4), + ('mjMESH_BUILTIN_SUPERTORUS', 4), ('mjMESH_BUILTIN_WEDGE', 5), ('mjMESH_BUILTIN_PLATE', 6), ]), diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index b90d95ea..b6d804c8 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -1041,6 +1041,16 @@ PYBIND11_MODULE(_specs, m) { } }, py::arg("nedge"), py::arg("radius")); + mjsMesh.def( + "make_supertorus", + [](raw::MjsMesh* self, int resolution, double radius, double s, + double t) { + double params[4] = {static_cast(resolution), radius, s, t}; + if (mjs_makeMesh(self, mjMESH_BUILTIN_SUPERTORUS, params, 4)) { + throw pybind11::value_error(mjs_getError(mjs_getSpec(self->element))); + } + }, + py::arg("resolution"), py::arg("radius"), py::arg("s"), py::arg("t")); mjsMesh.def( "make_plate", [](raw::MjsMesh* self, std::array& resolution) { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 44617bc4..65c25cd3 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -502,13 +502,17 @@ class SpecsTest(absltest.TestCase): mesh = spec.add_mesh(name='sphere') mesh.make_sphere(subdivision=2) + mesh = spec.add_mesh(name='supertoroid') + mesh.make_supertorus(resolution=10, radius=0.5, s=1, t=1) + model = spec.compile() - self.assertEqual(model.nmesh, 5) + self.assertEqual(model.nmesh, 6) 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) + self.assertEqual(model.mesh_vertnum[5], 100) 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 1a8f7db1..8aaa58a2 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -542,6 +542,35 @@ int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, int npar return 0; } + case mjMESH_BUILTIN_SUPERTORUS: { + if (nparams != 4) { + m->SetError(mjCError(0, "Supertorus mesh type requires 4 parameters")); + return -1; + } + int res = static_cast(params[0]); + if (res < 3) { + m->SetError(mjCError(0, "Supertorus resolution must be greater than 3")); + return -1; + } + double radius = params[1]; + if (radius <= 0 || radius > 1) { + m->SetError(mjCError(0, "Supertorus radius must be in (0, 1]")); + return -1; + } + double s = params[2]; + if (s <= 0) { + m->SetError(mjCError(0, "Supertorus 's' must be greater than 0")); + return -1; + } + double t = params[3]; + if (t <= 0) { + m->SetError(mjCError(0, "Supertorus 't' must be greater than 0")); + return -1; + } + meshC->MakeSupertorus(res, radius, s, t); + 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 ab124c40..26608368 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -76,6 +76,9 @@ namespace { using mujoco::user::FilePath; using std::max; using std::min; + using std::sin; + using std::cos; + using std::pow; // Parametrized linear/quintic interpolated nonlinearity. double Fovea(double x, double gamma) { @@ -84,7 +87,7 @@ namespace { // Foveal deformation. double g = mjMAX(0, mjMIN(1, gamma)); - return g * std::pow(x, 5) + (1 - g) * x; + return g * pow(x, 5) + (1 - g) * x; } // Evenly spaced numbers over a specified interval. @@ -120,18 +123,16 @@ namespace { // Transform spherical (azimuth, elevation, radius) to Cartesian (x,y,z). void SphericalToCartesian(const double aer[3], float xyz[3]) { double a = aer[0], e = aer[1], r = aer[2]; - xyz[0] = r * std::cos(e) * std::sin(a); - xyz[1] = r * std::sin(e); - xyz[2] = -r * std::cos(e) * std::cos(a); + xyz[0] = r * cos(e) * sin(a); + xyz[1] = r * sin(e); + xyz[2] = -r * cos(e) * cos(a); } // Tangent frame in Cartesian coordinates. void TangentFrame(const double aer[3], float mat[9]) { double a = aer[0], e = aer[1], r = aer[2]; - double ta[3] = {r * std::cos(e) * std::cos(a), 0, - r * std::cos(e) * std::sin(a)}; - double te[3] = {-r * std::sin(e) * std::sin(a), r * std::cos(e), - r * std::sin(e) * std::cos(a)}; + double ta[3] = {r * cos(e) * cos(a), 0, r * cos(e) * sin(a)}; + double te[3] = {-r * sin(e) * sin(a), r * cos(e), r * sin(e) * cos(a)}; double n[3]; mjuu_normvec(ta, 3); mjuu_normvec(te, 3); @@ -140,6 +141,14 @@ namespace { mjuu_crossvec(n, te, ta); mjuu_copyvec(mat, n, 3); } + + // parametric superellipsoid/supertoroid helper functions + double aux_c(double omega, double m) { + return std::copysign(pow(std::abs(cos(omega)), m), cos(omega)); + } + double aux_s(double omega, double m) { + return std::copysign(pow(std::abs(sin(omega)), m), sin(omega)); + } } // namespace // compute triangle area, surface normal, center @@ -2283,6 +2292,112 @@ void mjCMesh::MakeSphere(int subdiv, bool make_faces) { +// make a mesh of a torus (subsumed by supertorus, kept for reference only) +void mjCMesh::MakeTorus(int res, double radius) { + // allocate vertices and faces + int nvert = res * res; + int nface = res * res * 2; + std::vector vert(3 * nvert); + std::vector face(3 * nface); + + // generate vertices + for (int i = 0; i < res; ++i) { + for (int j = 0; j < res; ++j) { + double u = 2 * mjPI * i / res; + double v = 2 * mjPI * j / res; + int vidx = i * res + j; + vert[3 * vidx + 0] = (1 + radius * cos(v)) * cos(u); + vert[3 * vidx + 1] = (1 + radius * cos(v)) * sin(u); + vert[3 * vidx + 2] = radius * sin(v); + } + } + + // generate faces + int fidx = 0; + for (int i = 0; i < res; ++i) { + for (int j = 0; j < res; ++j) { + int i_next = (i + 1) % res; + int j_next = (j + 1) % res; + + int v1 = i * res + j; + int v2 = i_next * res + j; + int v3 = i_next * res + j_next; + int v4 = i * res + j_next; + + // first triangle + face[3 * fidx + 0] = v1; + face[3 * fidx + 1] = v2; + face[3 * fidx + 2] = v4; + fidx++; + + // second triangle + face[3 * fidx + 0] = v2; + face[3 * fidx + 1] = v3; + face[3 * fidx + 2] = v4; + fidx++; + } + } + + // save vertices and faces + mjs_setFloat(spec.uservert, vert.data(), vert.size()); + mjs_setInt(spec.userface, face.data(), face.size()); +} + + + +// make a mesh of a supertoroid, see https://en.wikipedia.org/wiki/Supertoroid +void mjCMesh::MakeSupertorus(int res, double radius, double s, double t) { + // allocate vertices and faces + int nvert = res * res; + int nface = res * res * 2; + std::vector vert(3 * nvert); + std::vector face(3 * nface); + + // generate vertices + for (int i = 0; i < res; ++i) { + for (int j = 0; j < res; ++j) { + double u = 2 * mjPI * i / res; + double v = 2 * mjPI * j / res; + int vidx = i * res + j; + vert[3 * vidx + 0] = (1 + radius * aux_c(v, s)) * aux_c(u, t); + vert[3 * vidx + 1] = (1 + radius * aux_c(v, s)) * aux_s(u, t); + vert[3 * vidx + 2] = radius * aux_s(v, s); + } + } + + // generate faces + int fidx = 0; + for (int i = 0; i < res; ++i) { + for (int j = 0; j < res; ++j) { + int i_next = (i + 1) % res; + int j_next = (j + 1) % res; + + int v1 = i * res + j; + int v2 = i_next * res + j; + int v3 = i_next * res + j_next; + int v4 = i * res + j_next; + + // first triangle + face[3 * fidx + 0] = v1; + face[3 * fidx + 1] = v2; + face[3 * fidx + 2] = v4; + fidx++; + + // second triangle + face[3 * fidx + 0] = v2; + face[3 * fidx + 1] = v3; + face[3 * fidx + 2] = v4; + fidx++; + } + } + + // save vertices and faces + mjs_setFloat(spec.uservert, vert.data(), vert.size()); + 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); @@ -2361,16 +2476,16 @@ void mjCMesh::MakeCone(int nedge, double radius) { // bottom face for (int i = 0; i < nedge; i++) { - uservert[3 * i + 0] = std::cos(2 * i * mjPI / nedge); - uservert[3 * i + 1] = std::sin(2 * i * mjPI / nedge); + uservert[3 * i + 0] = cos(2 * i * mjPI / nedge); + uservert[3 * i + 1] = sin(2 * i * mjPI / nedge); uservert[3 * i + 2] = -1; } // top face or single point if (radius > 0) { for (int i = nedge; i < 2 * nedge; i++) { - uservert[3 * i + 0] = radius * std::cos(2 * i * mjPI / nedge); - uservert[3 * i + 1] = radius * std::sin(2 * i * mjPI / nedge); + uservert[3 * i + 0] = radius * cos(2 * i * mjPI / nedge); + uservert[3 * i + 1] = radius * sin(2 * i * mjPI / nedge); uservert[3 * i + 2] = 1; } } else { @@ -3660,7 +3775,7 @@ void inline ComputeLinearStiffness(std::vector& K, double E, double nu) { // only linear elements are supported for now int order = 2; - int n = std::pow(order, 3); + int n = pow(order, 3); int ndof = 3*n; // compute quadrature points diff --git a/src/user/user_objects.h b/src/user/user_objects.h index cc00ea41..4ab174c0 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1053,6 +1053,8 @@ 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 MakeTorus(int res, double radius); + void MakeSupertorus(int res, double radius, double s, double t); void MakeWedge(int resolution[2], double fov[2], double gamma); void MakeRect(int resolution[2]); void MakeCone(int nedge, double radius); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 2f3f51c2..b0e28972 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -833,7 +833,7 @@ const mjMap meshbuiltin_map[meshbuiltin_sz] = { {"sphere", mjMESH_BUILTIN_SPHERE}, {"hemisphere", mjMESH_BUILTIN_HEMISPHERE}, {"cone", mjMESH_BUILTIN_CONE}, - {"torus", mjMESH_BUILTIN_TORUS}, + {"supertorus", mjMESH_BUILTIN_SUPERTORUS}, {"wedge", mjMESH_BUILTIN_WEDGE}, {"plate", mjMESH_BUILTIN_PLATE} }; diff --git a/test/user/testdata/makemesh.xml b/test/user/testdata/makemesh.xml index 19d61b4a..dcdaead8 100644 --- a/test/user/testdata/makemesh.xml +++ b/test/user/testdata/makemesh.xml @@ -2,6 +2,7 @@ + @@ -13,6 +14,7 @@ + diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 787ae3cd..057f3353 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -2040,24 +2040,6 @@ TEST_F(XMLReaderTest, ReadWedgeMesh) { mj_deleteModel(model); } -TEST_F(XMLReaderTest, UnsupportedMesh) { - static constexpr char xml[] = R"( - - - - - - - - - )"; - std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); - EXPECT_THAT(error.data(), HasSubstr("Unsupported mesh type")); - mj_deleteModel(model); -} - TEST_F(XMLReaderTest, BuiltinAndFile) { static constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index c8fca9f7..951aa60f 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -502,7 +502,7 @@ public enum mjtMeshBuiltin : int{ mjMESH_BUILTIN_SPHERE = 1, mjMESH_BUILTIN_HEMISPHERE = 2, mjMESH_BUILTIN_CONE = 3, - mjMESH_BUILTIN_TORUS = 4, + mjMESH_BUILTIN_SUPERTORUS = 4, mjMESH_BUILTIN_WEDGE = 5, mjMESH_BUILTIN_PLATE = 6, }