Add mjs_makeMesh to create a builtin mesh.

PiperOrigin-RevId: 788538127
Change-Id: I999733399a1a0da07b0f915f34d31364ab8b3b52

src/user/user_api.cc

@@ -16,6 +16,7 @@
 
 #include <algorithm>
 #include <cstddef>
+#include <cstring>
 #include <functional>
 #include <iterator>
 #include <map>
@@ -27,9 +28,9 @@
 
 #include <mujoco/mujoco.h>
 #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<mjCMesh*>(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<int>(params[0]),
+                         static_cast<int>(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<int>(params[0]),
+                         static_cast<int>(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<int>(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) {

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 --------------------------------------------
 

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<double> x_edges(resolution[0] + 1, 0);
+  std::vector<double> y_edges(resolution[1] + 1, 0);
+  BinEdges(x_edges.data(), y_edges.data(), resolution, fov, gamma);
+  std::vector<float> uservert(3 * resolution[0] * resolution[1], 0);
+  std::vector<float> 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<double> x_edges(resolution[0] + 1, 0);
+  std::vector<double> 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<float> uservert(3 * resolution[0] * resolution[1], 0);
+  std::vector<float> usernormal(9 * resolution[0] * resolution[1], 0);
+  std::vector<int> 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<float> 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() {

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_; }

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) {

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);
 

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<double> 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());
 }