Add built-in cone mesh type.

Also delete `prism` type since it is subsumed by cone.

PiperOrigin-RevId: 790992343
Change-Id: I7a9c6f2a4377489173acd64b23086c748d2cee8e

src/user/user_api.cc

@@ -505,62 +505,65 @@ mjsMaterial* mjs_addMaterial(mjSpec* s, const mjsDefault* defspec) {
 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;
+  switch (builtin) {
+    case 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;
     }
-    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;
+
+    case 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;
     }
-    if (fov[1] <= 0 || fov[1] > 90) {
-      m->SetError(
-          mjCError(0, "`fov[1]` must be a float between (0, 90] degrees"));
-      return -1;
+
+    case mjMESH_BUILTIN_CONE: {
+      if (nparams != 2) {
+        m->SetError(mjCError(0, "Cone mesh type requires 2 parameters"));
+        return -1;
+      }
+      int nedge = static_cast<int>(params[0]);
+      meshC->MakeCone(nedge, params[1]);
+      return 0;
     }
-    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;
+
+    default:
+      m->SetError(mjCError(0, "Unsupported mesh type"));
+      return 1;
   }
-  m->SetError(mjCError(0, "Unsupported mesh type"));
-  return 1;
 }
 
 // add pair to model

src/user/user_mesh.cc

@@ -184,6 +184,8 @@ static void ReadFromBuffer(T* dst, const char* src) {
   std::memcpy(dst, src, sizeof(T));
 }
 
+
+
 //------------------ class mjCMesh implementation --------------------------------------------------
 
 mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
@@ -432,6 +434,8 @@ void mjCMesh::LoadSDF() {
   delete[] field;
 }
 
+
+
 void mjCMesh::CacheMesh(mjCCache* cache, const mjResource* resource) {
   if (cache == nullptr) return;
 
@@ -521,6 +525,8 @@ struct VertexKey {
 
 }  // namespace
 
+
+
 // convert vertices to double precision and remove repeated vertices if requested
 void mjCMesh::ProcessVertices(const std::vector<float>& vert, bool remove_repeated) {
   vert_.clear();
@@ -622,6 +628,8 @@ bool mjCMesh::IsMSH() const {
   return content_type_ == "model/vnd.mujoco.msh";
 }
 
+
+
 // load mesh from resource; throw error on failure
 void mjCMesh::LoadFromResource(mjResource* resource, bool remove_repeated) {
   // set content type from resource name
@@ -645,6 +653,8 @@ void mjCMesh::LoadFromResource(mjResource* resource, bool remove_repeated) {
   }
 }
 
+
+
 // compiler wrapper
 void mjCMesh::Compile(const mjVFS* vfs) {
   try {
@@ -658,6 +668,8 @@ void mjCMesh::Compile(const mjVFS* vfs) {
   }
 }
 
+
+
 // compiler
 void mjCMesh::TryCompile(const mjVFS* vfs) {
   bool fromCache = false;
@@ -1106,6 +1118,8 @@ void mjCMesh::LoadOBJ(mjResource* resource, bool remove_repeated) {
   ProcessVertices(attrib.vertices, remove_repeated);
 }
 
+
+
 // load mesh from cached asset, return true on success
 bool mjCMesh::LoadCachedMesh(mjCCache *cache, const mjResource* resource) {
   auto process_mesh = [&](const void* data) {
@@ -1177,6 +1191,8 @@ bool mjCMesh::LoadCachedMesh(mjCCache *cache, const mjResource* resource) {
   return cache->PopulateData(resource, process_mesh);
 }
 
+
+
 // load STL binary mesh
 void mjCMesh::LoadSTL(mjResource* resource) {
   bool righthand = scale[0] * scale[1] * scale[2] > 0;
@@ -1339,6 +1355,8 @@ void mjCMesh::LoadMSH(mjResource* resource, bool remove_repeated) {
   ProcessVertices(vert, remove_repeated);
 }
 
+
+
 // compute the volume and center-of-mass of the mesh given the face centroid
 double mjCMesh::ComputeVolume(double CoM[3], const double facecen[3]) const {
   double normal[3], center[3], total_volume = 0;
@@ -1376,6 +1394,8 @@ double mjCMesh::ComputeVolume(double CoM[3], const double facecen[3]) const {
   return total_volume;
 }
 
+
+
 // compute the surface area and center-of-mass of the mesh given the face centroid
 double mjCMesh::ComputeSurfaceArea(double CoM[3], const double facecen[3]) const {
   double surface = 0;
@@ -1402,6 +1422,8 @@ double mjCMesh::ComputeSurfaceArea(double CoM[3], const double facecen[3]) const
   return surface;
 }
 
+
+
 // apply transformations
 void mjCMesh::ApplyTransformations() {
   // translate
@@ -1470,6 +1492,8 @@ void mjCMesh::ApplyTransformations() {
   }
 }
 
+
+
 // find centroid of faces, return total area
 double mjCMesh::ComputeFaceCentroid(double facecen[3]) const {
   double total_area = 0;
@@ -1496,6 +1520,8 @@ double mjCMesh::ComputeFaceCentroid(double facecen[3]) const {
   return total_area;
 }
 
+
+
 void mjCMesh::Process() {
   // create half-edge structure (if mesh was in XML)
   if (halfedge_.empty()) {
@@ -1737,6 +1763,7 @@ double mjCMesh::ComputeInertia(double inert[6], const double CoM[3]) const {
 }
 
 
+
 void mjCMesh::Rotate(double quat[4]) {
   // rotate vertices and normals of mesh by quaternion
   double neg[4] = {quat[0], -quat[1], -quat[2], -quat[3]};
@@ -1804,17 +1831,20 @@ void mjCMesh::CheckInitialMesh() const {
 
 
 
-// get inertia pointer
+// return inertia pointer
 double* mjCMesh::GetInertiaBoxPtr() {
   return boxsz_;
 }
 
 
+
+// return volume or surface area
 double mjCMesh::GetVolumeRef() const {
   return (inertia == mjMESH_INERTIA_SHELL) ? surface_ : volume_;
 }
 
 
+
 // make graph describing convex hull
 void mjCMesh::MakeGraph() {
   int adr, ok, curlong, totlong, exitcode;
@@ -1999,6 +2029,8 @@ void mjCMesh::MakeGraph() {
   }
 }
 
+
+
 // copy graph into face data
 void mjCMesh::CopyGraph() {
   // only if face data is missing
@@ -2022,6 +2054,8 @@ 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);
@@ -2047,6 +2081,8 @@ void mjCMesh::MakeWedge(int resolution[2], double fov[2], double gamma) {
                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);
@@ -2089,23 +2125,36 @@ void mjCMesh::MakeRect(int resolution[2]) {
              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);
 
+
+// make a mesh of a generalized discrete cone
+void mjCMesh::MakeCone(int nedge, double radius) {
+  int n = 3 * (nedge + (radius > 0 ? nedge : 1));
+  std::vector<float> uservert(n, 0);
+
+  // bottom face
   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);
-    }
+    uservert[3 * i + 0] = std::cos(2 * i * mjPI / nedge);
+    uservert[3 * i + 1] = std::sin(2 * i * mjPI / nedge);
+    uservert[3 * i + 2] = -1;
   }
 
-  mjs_setFloat(spec.uservert, uservert.data(), 3 * nedge * layer);
+  // 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 + 2] = 1;
+    }
+  } else {
+    uservert[3 * nedge + 2] = 1;
+  }
+
+  mjs_setFloat(spec.uservert, uservert.data(), n);
 }
 
+
+
 // compute vertex normals
 void mjCMesh::MakeNormal() {
   // only if normal data is missing
@@ -2435,8 +2484,7 @@ void MeshPolygon::InsertFace(int v1, int v2, int v3) {
 
 
 
-// return the traverse vertices of the polygon; there may be multiple paths if the polygon is
-// not connected
+// return the transverse vertices of the polygon, multiple paths possible if not connected
 std::vector<std::vector<int> > MeshPolygon::Paths() const {
   std::vector<std::vector<int> > paths;
   // shortcut if polygon is just a triangular face
@@ -2550,6 +2598,7 @@ void mjCMesh::MakePolygons() {
 }
 
 
+
 //------------------ class mjCSkin implementation --------------------------------------------------
 
 // constructor

src/user/user_objects.h

@@ -1053,7 +1053,7 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
   // 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);
+  void MakeCone(int nedge, double radius);
 
   // accessors
   const mjsPlugin& Plugin() const { return plugin; }