Add built-in supertoroid mesh.

PiperOrigin-RevId: 791180080
Change-Id: I3a70f50ff4a52a1bd4b88fc30334a574895145b4

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<int>(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"));

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<float> vert(3 * nvert);
+  std::vector<int> 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<float> vert(3 * nvert);
+  std::vector<int> 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<double> 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<double>& 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

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

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