From 89f47890859c4ffeb8749c26fb8eb57be489894a Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 29 Jul 2025 11:30:19 -0700 Subject: [PATCH] Add mjs_makeMesh to create a builtin mesh. PiperOrigin-RevId: 788538127 Change-Id: I999733399a1a0da07b0f915f34d31364ab8b3b52 --- doc/APIreference/functions.rst | 9 + doc/XMLreference.rst | 54 ++ doc/XMLschema.rst | 2 +- doc/changelog.rst | 1 + doc/includes/references.h | 11 + include/mujoco/mjspec.h | 10 + include/mujoco/mujoco.h | 2 + python/mujoco/introspect/enums.py | 15 + python/mujoco/introspect/functions.py | 28 + python/mujoco/specs.cc | 32 + python/mujoco/specs_test.py | 8 + src/user/user_api.cc | 65 +- src/user/user_api.h | 2 + src/user/user_mesh.cc | 152 ++++- src/user/user_objects.h | 5 + src/user/user_util.cc | 6 + src/user/user_util.h | 3 + src/xml/xml_native_reader.cc | 33 +- .../collision_convex/perf/spheremesh.xml | 644 +----------------- test/user/testdata/makemesh.xml | 13 + test/xml/xml_native_reader_test.cc | 112 +++ test/xml/xml_native_writer_test.cc | 1 + unity/Runtime/Bindings/MjBindings.cs | 10 + 23 files changed, 569 insertions(+), 649 deletions(-) create mode 100644 test/user/testdata/makemesh.xml diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 21885e9d..ba2074f8 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -4074,6 +4074,15 @@ Add texture. Add material. +.. _mjs_makeMesh: + +`mjs_makeMesh <#mjs_makeMesh>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mjs_makeMesh + +Sets the vertices and normals of a mesh. + .. _FindAndGetUtilities: Find and get utilities diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 8c4b9431..3d79acf7 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1323,6 +1323,60 @@ The full list of processing steps applied by the compiler to each mesh is as fol Reference orientation relative to which the 3D vertex coordinates and normals are defined. The conjugate of this quaternion is used to rotate the positions and normals. The model compiler normalizes the quaternion automatically. +.. _asset-mesh-builtin: + +:at:`builtin`: :at-val:`string, optional` + The mesh is generated by the compiler from a set of parameters specified in :ref:`params`. + When saved to XML, meshes produced this way are converted to explicit vertices. + The available built-in types, their parameters and semantics are: + + :at-val:`sphere` (subdivision) + Repeated subdivisions :math:`s` of a unit icosahedron. Has :math:`2 + 10 \cdot 4^s` vertices. + + **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. + + **subdivision**: integer in [0-10]: The number of subdivisions to apply to the pyramid. + + :at-val:`prism` (nvert) + An extruded regular unit polygon. + + **nvert**: integer >= 3: The number vertices in the polygon. + + :at-val:`cone` (nvert) + The convex hull of a regular unit polygon and the vertex (0, 0, 1). + + **nvert**: integer >= 3: The number vertices in the polygon. + + :at-val:`torus` (radius, resolution) + A torus with major radius of 1 and given minor radius. + + **radius**: real in (0, 1]: The minor radius of the torus. + |br| **resolution** integer >= 4: The discretization of both major and minor radii. + + :at-val:`wedge` (res_phi, res_theta, fov_phi, fov_theta, gamma) + A slice of a unit spherical shell in spherical coordinates. + + **res_phi**: integer >= 0: The vertical resolution of the slice. + |br| **res_theta**: integer >= 0: The horizontal resolution of the slice. + |br| **fov_phi**: real in (0, 180]: The horizontal field of view (longitude) in degrees. + |br| **fov_phi**: real in (0, 90): The vertical field of view (latitude) in degrees. + |br| **gamma**: real in [0, 1]: Foveal deformation of the discretization. + + :at-val:`plate` (res_x, res_y) + A rectangular plate with resolution in each dimension. + + **res_x**: integer > 0: The horizontal resolution of the plate. + |br| **res_y**: integer > 0: The vertical resolution of the plate. + +.. _asset-mesh-params: + +:at:`params`: :at-val:`real(nparam), optional` + The parameters used to generate a builtin mesh. The number and type of parameters and their semantic depends on the + mesh type. See :ref:`mesh/builtin` for details. + .. _mesh-plugin: :el-prefix:`mesh/` |-| **plugin** (?) diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 23d05a96..c01d2b48 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -103,7 +103,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`refpos` | :ref:`refquat` | :ref:`scale` | :ref:`smoothnormal` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`maxhullvert` | :ref:`inertia` | | | | +| | | | :ref:`maxhullvert` | :ref:`inertia` | :ref:`builtin` | :ref:`params` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_2| mesh |br| |_2| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 32fa7595..e2f5b530 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -15,6 +15,7 @@ General - Removed the SdfLib plugin and the dependency on `SdfLib `__. SDFs are now supported natively in mjModel. - Removed ``oct_depth`` from :ref:`mjvOption` (unused). +- Added the functionality to create a builtin meshes, see :ref:`mesh/builtin`. - Inertia computation in MuJoCo C is now performed by a new :ref:`pipeline` function :ref:`mj_makeM`, which combines the Composite Rigid Body algorithm in :ref:`mj_crb` and additional terms related to :ref:`tendon armature`. Code that uses :ref:`mj_crb` to compute the inertia should now use diff --git a/doc/includes/references.h b/doc/includes/references.h index c711b5aa..1281f73b 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1781,6 +1781,16 @@ typedef enum mjtMeshInertia_ { // type of mesh inertia mjMESH_INERTIA_LEGACY, // legacy mesh inertia mjMESH_INERTIA_SHELL // shell mesh inertia } mjtMeshInertia; +typedef enum mjtMeshBuiltin_ { // type of built-in procedural mesh + mjMESH_BUILTIN_NONE = 0, // no built-in mesh + mjMESH_BUILTIN_SPHERE, // sphere + mjMESH_BUILTIN_HEMISPHERE, // hemisphere + mjMESH_BUILTIN_PRISM, // prism + mjMESH_BUILTIN_CONE, // cone + mjMESH_BUILTIN_TORUS, // torus + mjMESH_BUILTIN_WEDGE, // wedge + mjMESH_BUILTIN_PLATE, // plate +} mjtMeshBuiltin; typedef enum mjtBuiltin_ { // type of built-in procedural texture mjBUILTIN_NONE = 0, // no built-in texture mjBUILTIN_GRADIENT, // gradient: rgb1->rgb2 @@ -3457,6 +3467,7 @@ mjsHField* mjs_addHField(mjSpec* s); mjsSkin* mjs_addSkin(mjSpec* s); mjsTexture* mjs_addTexture(mjSpec* s); mjsMaterial* mjs_addMaterial(mjSpec* s, const mjsDefault* def); +int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, int nparams); mjSpec* mjs_getSpec(mjsElement* element); mjSpec* mjs_findSpec(mjSpec* spec, const char* name); mjsBody* mjs_findBody(mjSpec* s, const char* name); diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 08f8a328..8dbc0f1a 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -70,6 +70,16 @@ typedef enum mjtMeshInertia_ { // type of mesh inertia mjMESH_INERTIA_SHELL // shell mesh inertia } mjtMeshInertia; +typedef enum mjtMeshBuiltin_ { // type of built-in procedural mesh + mjMESH_BUILTIN_NONE = 0, // no built-in mesh + mjMESH_BUILTIN_SPHERE, // sphere + mjMESH_BUILTIN_HEMISPHERE, // hemisphere + mjMESH_BUILTIN_PRISM, // prism + mjMESH_BUILTIN_CONE, // cone + mjMESH_BUILTIN_TORUS, // torus + mjMESH_BUILTIN_WEDGE, // wedge + mjMESH_BUILTIN_PLATE, // plate +} mjtMeshBuiltin; typedef enum mjtBuiltin_ { // type of built-in procedural texture mjBUILTIN_NONE = 0, // no built-in texture diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 778cfc8a..9ab1d2ca 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1545,6 +1545,8 @@ MJAPI mjsTexture* mjs_addTexture(mjSpec* s); // Add material. MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, const mjsDefault* def); +// Sets the vertices and normals of a mesh. +MJAPI int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, int nparams); //---------------------------------- Find and get utilities ---------------------------------------- diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index f49661bc..74ac1167 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -791,6 +791,21 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjMESH_INERTIA_SHELL', 3), ]), )), + ('mjtMeshBuiltin', + EnumDecl( + name='mjtMeshBuiltin', + declname='enum mjtMeshBuiltin_', + values=dict([ + ('mjMESH_BUILTIN_NONE', 0), + ('mjMESH_BUILTIN_SPHERE', 1), + ('mjMESH_BUILTIN_HEMISPHERE', 2), + ('mjMESH_BUILTIN_PRISM', 3), + ('mjMESH_BUILTIN_CONE', 4), + ('mjMESH_BUILTIN_TORUS', 5), + ('mjMESH_BUILTIN_WEDGE', 6), + ('mjMESH_BUILTIN_PLATE', 7), + ]), + )), ('mjtBuiltin', EnumDecl( name='mjtBuiltin', diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 4070d123..b9bb91e2 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -9869,6 +9869,34 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Add material.', )), + ('mjs_makeMesh', + FunctionDecl( + name='mjs_makeMesh', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='mesh', + type=PointerType( + inner_type=ValueType(name='mjsMesh'), + ), + ), + FunctionParameterDecl( + name='builtin', + type=ValueType(name='mjtMeshBuiltin'), + ), + FunctionParameterDecl( + name='params', + type=PointerType( + inner_type=ValueType(name='double'), + ), + ), + FunctionParameterDecl( + name='nparams', + type=ValueType(name='int'), + ), + ), + doc='Sets the vertices and normals of a mesh.', + )), ('mjs_getSpec', FunctionDecl( name='mjs_getSpec', diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 3067e385..550fa7a1 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -1001,6 +1001,38 @@ PYBIND11_MODULE(_specs, m) { mjs_setDefault(self.element, &default_); }, py::return_value_policy::reference_internal); + mjsMesh.def( + "make_wedge", + [](raw::MjsMesh* self, std::array& resolution, double radius, + std::array& fov, double gamma) { + double params[5] = {static_cast(resolution[0]), + static_cast(resolution[1]), fov[0], fov[1], + gamma}; + if (mjs_makeMesh(self, mjMESH_BUILTIN_WEDGE, params, 5)) { + throw pybind11::value_error(mjs_getError(mjs_getSpec(self->element))); + } + }, + py::arg("resolution") = std::array{0, 0}, py::arg("radius"), + py::arg("fov") = std::array{0, 0}, py::arg("gamma") = 0); + mjsMesh.def( + "make_prism", + [](raw::MjsMesh* self, int nedge) { + double params[1] = {static_cast(nedge)}; + if (mjs_makeMesh(self, mjMESH_BUILTIN_PRISM, params, 1)) { + throw pybind11::value_error(mjs_getError(mjs_getSpec(self->element))); + } + }, + py::arg("nedge")); + mjsMesh.def( + "make_plate", + [](raw::MjsMesh* self, std::array& resolution) { + double params[2] = {static_cast(resolution[0]), + static_cast(resolution[1])}; + if (mjs_makeMesh(self, mjMESH_BUILTIN_PLATE, params, 2)) { + throw pybind11::value_error(mjs_getError(mjs_getSpec(self->element))); + } + }, + py::arg("resolution") = std::array{0, 0}); // ============================= MJSPAIR ===================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsPair& obj) { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 4e8e2dcc..8d3d3fc5 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -484,6 +484,14 @@ class SpecsTest(absltest.TestCase): # Check that the state is the same. np.testing.assert_array_equal(state1, state2) + def test_make_mesh(self): + spec = mujoco.MjSpec() + mesh = spec.add_mesh(name='wedge') + mesh.make_wedge(resolution=[25, 25], radius=1, fov=[90, 45], gamma=0) + model = spec.compile() + self.assertEqual(model.nmesh, 1) + self.assertEqual(model.nmeshvert, 25 * 25) + 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 b8890521..62efd6e5 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -27,9 +28,9 @@ #include #include "engine/engine_support.h" +#include "user/user_cache.h" #include "user/user_model.h" #include "user/user_objects.h" -#include "user/user_cache.h" #include "user/user_util.h" namespace { @@ -500,7 +501,67 @@ mjsMaterial* mjs_addMaterial(mjSpec* s, const mjsDefault* defspec) { return &material->spec; } - +// Sets the vertices and normals of a mesh. +int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, int nparams) { + mjCMesh* meshC = static_cast(mesh->element); + mjCModel* m = meshC->model; + if (builtin == mjMESH_BUILTIN_WEDGE) { + if (nparams != 5) { + m->SetError(mjCError(0, "Wedge builtin mesh types require 5 parameters")); + return -1; + } + int resolution[2] = {static_cast(params[0]), + static_cast(params[1])}; + double fov[2] = {params[2], params[3]}; + double gamma = params[4]; + if (fov[0] <= 0 || fov[0] > 180) { + m->SetError( + mjCError(0, "fov[0] must be a float between (0, 180] degrees")); + return -1; + } + if (fov[1] <= 0 || fov[1] > 90) { + m->SetError( + mjCError(0, "`fov[1]` must be a float between (0, 90] degrees")); + return -1; + } + if (resolution[0] <= 0 || resolution[1] <= 0) { + m->SetError( + mjCError(0, "Horizontal and vertical resolutions must be positive")); + return -1; + } + if (gamma < 0 || gamma > 1) { + m->SetError( + mjCError(0, "`gamma` must be a nonnegative float between [0, 1]")); + return -1; + } + meshC->MakeWedge(resolution, fov, gamma); + return 0; + } else if (builtin == mjMESH_BUILTIN_PLATE) { + if (nparams != 2) { + m->SetError(mjCError(0, "Plate builtin mesh type requires 2 parameters")); + return -1; + } + int resolution[2] = {static_cast(params[0]), + static_cast(params[1])}; + if (resolution[0] <= 0 || resolution[1] <= 0) { + m->SetError( + mjCError(0, "Horizontal and vertical resolutions must be positive")); + return -1; + } + meshC->MakeRect(resolution); + return 0; + } else if (builtin == mjMESH_BUILTIN_PRISM) { + if (nparams != 1) { + m->SetError(mjCError(0, "Prism mesh type requires 1 parameter")); + return -1; + } + int nedge = static_cast(params[0]); + meshC->MakePrism(nedge); + return 0; + } + m->SetError(mjCError(0, "Unsupported mesh type")); + return 1; +} // add pair to model mjsPair* mjs_addPair(mjSpec* s, const mjsDefault* defspec) { diff --git a/src/user/user_api.h b/src/user/user_api.h index f5b5c059..0be8ba1f 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -209,6 +209,8 @@ MJAPI mjsTexture* mjs_addTexture(mjSpec* s); // Add material. MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, const mjsDefault* def); +// Sets the vertices and normals of a mesh. +MJAPI int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, int nparams); //---------------------------------- Find/get utilities -------------------------------------------- diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index a84a9220..745e9c5a 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -75,6 +75,70 @@ namespace { using mujoco::user::FilePath; using std::max; using std::min; + + // Parametrized linear/quintic interpolated nonlinearity. + double Fovea(double x, double gamma) { + // Quick return. + if (!gamma) return x; + + // Foveal deformation. + double g = mjMAX(0, mjMIN(1, gamma)); + return g * std::pow(x, 5) + (1 - g) * x; + } + + // Evenly spaced numbers over a specified interval. + void LinSpace(double lower, double upper, int n, double array[]) { + double increment = n > 1 ? (upper - lower) / (n - 1) : 0; + for (int i = 0; i < n; ++i) { + *array = lower; + ++array; + lower += increment; + } + } + + // Make bin edges. + void BinEdges(double* x_edges, double* y_edges, int size[2], double fov[2], + double gamma) { + // Make unit bin edges. + LinSpace(-1, 1, size[0] + 1, x_edges); + LinSpace(-1, 1, size[1] + 1, y_edges); + + // Apply foveal deformation. + for (int i = 0; i < size[0] + 1; i++) { + x_edges[i] = Fovea(x_edges[i], gamma); + } + for (int i = 0; i < size[1] + 1; i++) { + y_edges[i] = Fovea(y_edges[i], gamma); + } + + // Scale by field-of-view. + mjuu_scalevec(x_edges, x_edges, fov[0] * mjPI / 180, size[0] + 1); + mjuu_scalevec(y_edges, y_edges, fov[1] * mjPI / 180, size[1] + 1); + } + + // 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); + } + + // 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 n[3]; + mjuu_normvec(ta, 3); + mjuu_normvec(te, 3); + mjuu_copyvec(mat + 3, ta, 3); + mjuu_copyvec(mat + 6, te, 3); + mjuu_crossvec(n, te, ta); + mjuu_copyvec(mat, n, 3); + } } // namespace // compute triangle area, surface normal, center @@ -1488,7 +1552,11 @@ void mjCMesh::Process() { // facenormal might not exist if usernormal was specified if (facenormal_.empty()) { - facenormal_ = face_; + int normal_per_vertex = normal_.size() / vert_.size(); + facenormal_.assign(face_.size(), 0); + for (int i = 0; i < face_.size(); i++) { + facenormal_[i] = normal_per_vertex * face_[i]; + } } MakePolygons(); @@ -1954,7 +2022,89 @@ void mjCMesh::CopyGraph() { } } +// 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); + std::vector y_edges(resolution[1] + 1, 0); + BinEdges(x_edges.data(), y_edges.data(), resolution, fov, gamma); + std::vector uservert(3 * resolution[0] * resolution[1], 0); + std::vector usernormal(9 * resolution[0] * resolution[1], 0); + for (int i = 0; i < resolution[0]; i++) { + for (int j = 0; j < resolution[1]; j++) { + double aer[3]; + aer[0] = 0.5 * (x_edges[i + 1] + x_edges[i]); + aer[1] = 0.5 * (y_edges[j + 1] + y_edges[j]); + aer[2] = 1; + SphericalToCartesian(aer, uservert.data() + 3 * (i * resolution[1] + j)); + TangentFrame(aer, usernormal.data() + 9 * (i * resolution[1] + j)); + } + } + + mjs_setFloat(spec.uservert, uservert.data(), + 3 * resolution[0] * resolution[1]); + mjs_setFloat(spec.usernormal, usernormal.data(), + 9 * resolution[0] * resolution[1]); +} + +// make a mesh of a rectangle +void mjCMesh::MakeRect(int resolution[2]) { + std::vector x_edges(resolution[0] + 1, 0); + std::vector y_edges(resolution[1] + 1, 0); + LinSpace(-1, 1, resolution[0] + 1, x_edges.data()); + LinSpace(-1, 1, resolution[1] + 1, y_edges.data()); + std::vector uservert(3 * resolution[0] * resolution[1], 0); + std::vector usernormal(9 * resolution[0] * resolution[1], 0); + std::vector userface(6 * (resolution[0] - 1) * (resolution[1] - 1), 0); + spec.inertia = mjMESH_INERTIA_SHELL; + + for (int i = 0; i < resolution[0]; i++) { + for (int j = 0; j < resolution[1]; j++) { + int vert = i * resolution[1] + j; + mjtNum dx = 2. / resolution[0]; + mjtNum dy = 2. / resolution[1]; + uservert[3 * vert + 0] = -1 + (i + 0.5) * dx; + uservert[3 * vert + 1] = -1 + (j + 0.5) * dy; + uservert[3 * vert + 2] = -1; + usernormal[9 * vert + 0] = 1; + usernormal[9 * vert + 4] = 1; + usernormal[9 * vert + 8] = 1; + if (i > 0 && j > 0) { + int cell = (i - 1) * (resolution[1] - 1) + j - 1; + userface[6 * cell + 0] = (i - 1) * resolution[1] + j - 1; + userface[6 * cell + 1] = (i - 0) * resolution[1] + j - 1; + userface[6 * cell + 2] = (i - 1) * resolution[1] + j - 0; + userface[6 * cell + 3] = (i - 0) * resolution[1] + j - 0; + userface[6 * cell + 4] = (i - 1) * resolution[1] + j - 0; + userface[6 * cell + 5] = (i - 0) * resolution[1] + j - 1; + } + } + } + + mjs_setFloat(spec.uservert, uservert.data(), + 3 * resolution[0] * resolution[1]); + mjs_setFloat(spec.usernormal, usernormal.data(), + 9 * resolution[0] * resolution[1]); + mjs_setInt(spec.userface, userface.data(), + 6 * (resolution[0] - 1) * (resolution[1] - 1)); +} + +// make a mesh of a prism +void mjCMesh::MakePrism(int nedge) { + int layer = 2; + std::vector uservert(3 * nedge * layer, 0); + + for (int i = 0; i < nedge; i++) { + for (int j = 0; j < layer; j++) { + int v = i * layer + j; + uservert[3 * v + 0] = std::cos(2 * i * mjPI / nedge); + uservert[3 * v + 1] = std::sin(2 * i * mjPI / nedge); + uservert[3 * v + 2] = -1 + 2 * j / (layer - 1); + } + } + + mjs_setFloat(spec.uservert, uservert.data(), 3 * nedge * layer); +} // compute vertex normals void mjCMesh::MakeNormal() { diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 56d5b01d..7f3a7307 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1050,6 +1050,11 @@ class mjCMesh: public mjCMesh_, private mjsMesh { void PointToLocal(void); void NameSpace(const mjCModel* m); + // make a mesh of a predefined shape + void MakeWedge(int resolution[2], double fov[2], double gamma); + void MakeRect(int resolution[2]); + void MakePrism(int nedge); + // accessors const mjsPlugin& Plugin() const { return plugin; } const std::string& ContentType() const { return content_type_; } diff --git a/src/user/user_util.cc b/src/user/user_util.cc index 87ad738d..5df993d4 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -192,6 +192,12 @@ float mjuu_normvec(float* vec, const int n) { return nrm; } +// scale vector by scalar +void mjuu_scalevec(double* res, const double* vec, double s, int n) { + for (int i = 0; i < n; i++) { + res[i] = s * vec[i]; + } +} // convert quaternion to rotation matrix void mjuu_quat2mat(double* res, const double* quat) { diff --git a/src/user/user_util.h b/src/user/user_util.h index 98ebb40c..31abf544 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -73,6 +73,9 @@ double mjuu_L1(const double* a, const double* b, int n); double mjuu_normvec(double* vec, int n); float mjuu_normvec(float* vec, int n); +// scale vector by scalar +void mjuu_scalevec(double* res, const double* vec, double s, int n); + // convert quaternion to rotation matrix void mjuu_quat2mat(double* res, const double* quat); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 5e480f24..79631004 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -230,9 +230,9 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"asset", "*", "0"}, {"<"}, - {"mesh", "*", "14", "name", "class", "content_type", "file", "vertex", "normal", + {"mesh", "*", "16", "name", "class", "content_type", "file", "vertex", "normal", "texcoord", "face", "refpos", "refquat", "scale", "smoothnormal", - "maxhullvert", "inertia"}, + "maxhullvert", "inertia", "builtin", "params"}, {"<"}, {"plugin", "*", "2", "plugin", "instance"}, {"<"}, @@ -825,6 +825,20 @@ const mjMap meshinertia_map[4] = { }; +// mesh builtin type +const int meshbuiltin_sz = 8; +const mjMap meshbuiltin_map[meshbuiltin_sz] = { + {"none", mjMESH_BUILTIN_NONE}, + {"sphere", mjMESH_BUILTIN_SPHERE}, + {"hemisphere", mjMESH_BUILTIN_HEMISPHERE}, + {"prism", mjMESH_BUILTIN_PRISM}, + {"cone", mjMESH_BUILTIN_CONE}, + {"torus", mjMESH_BUILTIN_TORUS}, + {"wedge", mjMESH_BUILTIN_WEDGE}, + {"plate", mjMESH_BUILTIN_PLATE} +}; + + // flexcomp type const mjMap fcomp_map[mjNFCOMPTYPES] = { {"grid", mjFCOMPTYPE_GRID}, @@ -1539,6 +1553,21 @@ void mjXReader::OneMesh(XMLElement* elem, mjsMesh* mesh, const mjVFS* vfs) { } } + // read builtin options + if (MapValue(elem, "builtin", &n, meshbuiltin_map, meshbuiltin_sz)) { + std::vector params; + int nparams = ReadVector(elem, "params", params, text, /*required*/ true); + if (file) { + throw mjXError(elem, "builtin cannot be used with a mesh file"); + } + if (!mesh->uservert->empty()) { + throw mjXError(elem, "builtin mesh cannot be used with user vertex data"); + } + if (mjs_makeMesh(mesh, (mjtMeshBuiltin)n, params.data(), nparams)) { + throw mjXError(elem, mjs_getError(spec)); + } + } + // write error info mjs_setString(mesh->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); } diff --git a/test/engine/testdata/collision_convex/perf/spheremesh.xml b/test/engine/testdata/collision_convex/perf/spheremesh.xml index 16a7c18b..f6e747ad 100644 --- a/test/engine/testdata/collision_convex/perf/spheremesh.xml +++ b/test/engine/testdata/collision_convex/perf/spheremesh.xml @@ -40,648 +40,6 @@ - + diff --git a/test/user/testdata/makemesh.xml b/test/user/testdata/makemesh.xml new file mode 100644 index 00000000..00c977e1 --- /dev/null +++ b/test/user/testdata/makemesh.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 05a6c61c..858c305a 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -2021,6 +2021,118 @@ TEST_F(XMLReaderTest, ReadShellParameter) { mj_deleteModel(model); } +// ----------------------- test builtin mesh parsing --------------------------- + +TEST_F(XMLReaderTest, ReadWedgeMesh) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + 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"( + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), + HasSubstr("builtin mesh cannot be used with user vertex data")); + mj_deleteModel(model); +} + +TEST_F(XMLReaderTest, MakePlateNoParameters) { + 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("required attribute missing: 'params'")); + mj_deleteModel(model); +} + +TEST_F(XMLReaderTest, MakePlateTooFewParameters) { + 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("Plate builtin mesh type requires 2 parameters")); + mj_deleteModel(model); +} + +TEST_F(XMLReaderTest, MakePlateInvalidParameters) { + 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("resolutions must be positive")); + mj_deleteModel(model); +} + +// ----------------------- test skin parsing -------------------------------- + TEST_F(XMLReaderTest, ReadsSkinGroups) { static constexpr char xml[] = R"( diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 6876245a..134dd90e 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -1386,6 +1386,7 @@ TEST_F(XMLWriterTest, WriteReadCompare) { absl::StrContains(p.path().string(), "shark_") || absl::StrContains(p.path().string(), "spheremesh") || // exclude files that fail the comparison test + absl::StrContains(p.path().string(), "makemesh") || absl::StrContains(p.path().string(), "usd") || absl::StrContains(p.path().string(), "torus_maxhull") || absl::StrContains(p.path().string(), "fitmesh_") || diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index c2c8f65b..c0432de1 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -496,6 +496,16 @@ public enum mjtMeshInertia : int{ mjMESH_INERTIA_LEGACY = 2, mjMESH_INERTIA_SHELL = 3, } +public enum mjtMeshBuiltin : int{ + mjMESH_BUILTIN_NONE = 0, + mjMESH_BUILTIN_SPHERE = 1, + mjMESH_BUILTIN_HEMISPHERE = 2, + mjMESH_BUILTIN_PRISM = 3, + mjMESH_BUILTIN_CONE = 4, + mjMESH_BUILTIN_TORUS = 5, + mjMESH_BUILTIN_WEDGE = 6, + mjMESH_BUILTIN_PLATE = 7, +} public enum mjtBuiltin : int{ mjBUILTIN_NONE = 0, mjBUILTIN_GRADIENT = 1,