Convert internal representation of vertices to double. Also replace remove duplicate vertices with fast hash implementation.

PiperOrigin-RevId: 734516595
Change-Id: Ic78556688d5f2b75d69cd286b8d060e2cdeb7c48

doc/XMLreference.rst

@@ -3397,10 +3397,11 @@ saving the XML:
    compatible with :at:`dim=1`.
 
    **mesh** loads the flexcomp points and elements (i.e. triangles) from a mesh file, in the same file formats as mesh
-   assets. A mesh asset is not actually added to the model. Instead the vertex and face data from the mesh file are used
+   assets, excluding the legacy .msh format. A mesh asset is not actually added to the model. Instead the vertex and
-   to populate the point and element data of the flexcomp. :at:`dim` is automatically set to 2. Recall that a mesh asset
+   face data from the mesh file are used to populate the point and element data of the flexcomp. :at:`dim` is
-   in MuJoCo can be used as a rigid geom attached to a single body. In contrast, the flex generated here corresponds to
+   automatically set to 2. Recall that a mesh asset in MuJoCo can be used as a rigid geom attached to a single body. In
-   a soft mesh with the same initial shape, where each vertex is a separate moving body (unless pinned).
+   contrast, the flex generated here corresponds to a soft mesh with the same initial shape, where each vertex is a
+   separate moving body (unless pinned).
 
    .. _gmsh-file-docs:
 
@@ -3471,9 +3472,9 @@ saving the XML:
 
 :at:`file`: :at-val:`string, optional`
    The name of the file from which a **surface** (triangular) or **volumetric** (tetrahedral) mesh is loaded. For
-   surface meshes, the file extension is used to determine the file format. Supported formats are the same as in
+   surface meshes, the file extension is used to determine the file format. Supported formats are GMSH and the formats
-   :ref:`mesh assets<asset-mesh>` and also including GMSH. Volumetric meshes are supported only in GMSH format.
+   specified in :ref:`mesh assets<asset-mesh>`, excluding the legacy .msh format. Volumetric meshes are supported only
-   See :ref:`here<gmsh-file-docs>` for more information on GMSH files.
+   in GMSH format. See :ref:`here<gmsh-file-docs>` for more information on GMSH files.
 
 .. _body-flexcomp-rigid:
 

src/user/user_flexcomp.cc

@@ -989,14 +989,6 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
     return comperr(error, "File is required", error_sz);
   }
 
-  // get extension and check; must be STL, OBJ or MSH
-  std::string ext = mjuu_getext(file);
-  if (strcasecmp(ext.c_str(), ".stl") &&
-      strcasecmp(ext.c_str(), ".obj") &&
-      strcasecmp(ext.c_str(), ".msh")) {
-    return comperr(error, "Mesh file extension must be stl, obj or msh", error_sz);
-  }
-
   // check dim
   if (def.spec.flex->dim < 2) {
     return comperr(error, "Flex dim must be at least 2 for mesh", error_sz);
@@ -1006,6 +998,11 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
   std::string filename = mjuu_combinePaths(mjs_getString(model->spec.meshdir), file);
   mjResource* resource = nullptr;
 
+
+  if (mjCMesh::IsMSH(filename)) {
+    return comperr(error, "legacy MSH files are not supported in flexcomp", error_sz);
+  }
+
   try {
     resource = mjCBase::LoadResource(mjs_getString(model->spec.modelfiledir),
                                      filename, 0);
@@ -1013,46 +1010,24 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
     return comperr(error, err.message, error_sz);
   }
 
+
   // load mesh
   mjCMesh mesh;
-  bool isobj = false;
   try {
-    if (!strcasecmp(ext.c_str(), ".stl")) {
+    mesh.LoadFromResource(resource, true);
-      mesh.LoadSTL(resource);
-    } else if (!strcasecmp(ext.c_str(), ".obj")) {
-      isobj = true;
-      mesh.LoadOBJ(resource);
-    } else {
-      mesh.LoadMSH(resource);
-    }
     mju_closeResource(resource);
   } catch (mjCError err) {
     mju_closeResource(resource);
     return comperr(error, err.message, error_sz);
   }
 
-  // LoadOBJ uses userXXX, extra processing needed
+  // check sizes
-  if (isobj) {
+  if (mesh.Vert().empty() || mesh.Face().empty()) {
-    // check sizes
+    return comperr(error, "Vertex and face data required", error_sz);
-    if (mesh.Vert().empty() || mesh.Face().empty()) {
-      return comperr(error, "Vertex and face data required", error_sz);
-    }
-    if (mesh.Vert().size()%3) {
-      return comperr(error, "Vertex data must be multiple of 3", error_sz);
-    }
-    if (mesh.Face().size()%3) {
-      return comperr(error, "Face data must be multiple of 3", error_sz);
-    }
-
-    // remove repeated vertices (not called in LoadOBJ)
-    mesh.RemoveRepeated();
   }
 
-  // copy vertices, convert from float to double
+  // copy vertices
-  point = vector<double> (mesh.nvert()*3);
+  point = mesh.Vert();
-  for (int i=0; i < mesh.nvert()*3; i++) {
-    point[i] = (double) mesh.Vert(i);
-  }
 
   if (mesh.HasTexcoord()) {
     texcoord = mesh.Texcoord();

src/user/user_objects.cc

@@ -734,7 +734,7 @@ std::string mjCBase::GetAssetContentType(std::string_view resource_name,
     auto type = mjuu_parseContentTypeAttrType(raw_text);
     auto subtype = mjuu_parseContentTypeAttrSubtype(raw_text);
     if (!type.has_value() || !subtype.has_value()) {
-      throw mjCError(this, "invalid format for content_type");
+      return "";
     }
     return std::string(*type) + "/" + std::string(*subtype);
   } else {

src/user/user_objects.h

@@ -15,6 +15,7 @@
 #ifndef MUJOCO_SRC_USER_USER_OBJECTS_H_
 #define MUJOCO_SRC_USER_USER_OBJECTS_H_
 
+#include <stdbool.h>
 #include <cstddef>
 #include <array>
 #include <cstdlib>
@@ -195,7 +196,7 @@ class mjCBase : public mjCBase_ {
 
   // Get and sanitize content type from raw_text if not empty, otherwise parse
   // content type from resource_name; throw on failure
-  std::string GetAssetContentType(std::string_view resource_name, std::string_view raw_text);
+  static std::string GetAssetContentType(std::string_view resource_name, std::string_view raw_text);
 
   // Add frame transformation
   void SetFrame(mjCFrame* _frame);
@@ -842,9 +843,10 @@ class mjCMesh_ : public mjCBase {
   std::string plugin_name;
   std::string plugin_instance_name;
 
-  std::string content_type_;                     // content type of file
+  std::string content_type_ = "";                // content type of file
   std::string file_;                             // mesh file
-  std::vector<float> vert_;                      // vertex data
+  mjResource* resource_ = nullptr;               // resource for mesh file
+  std::vector<double> vert_;                      // vertex data
   std::vector<float> normal_;                    // normal data
   std::vector<float> texcoord_;                  // texcoord data
   std::vector<int> face_;                        // vertex indices
@@ -886,7 +888,7 @@ class mjCMesh_ : public mjCBase {
   double surface_;                    // surface of the mesh
 
   // size of mesh data to be copied into mjModel
-  int szgraph_;                       // size of graph data in ints
+  int szgraph_ = 0;                   // size of graph data in ints
   bool needhull_;                     // needs convex hull for collisions
   int maxhullvert_;                   // max vertex count of convex hull
 
@@ -923,8 +925,8 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
   const double* Refquat() const { return refquat; }
   const double* Scale() const { return scale; }
   bool SmoothNormal() const { return smoothnormal; }
-  const std::vector<float>& Vert() const { return vert_; }
+  const std::vector<double>& Vert() const { return vert_; }
-  float Vert(int i) const { return vert_[i]; }
+  double Vert(int i) const { return vert_[i]; }
   const std::vector<float>& UserVert() const { return spec_vert_; }
   const std::vector<float>& UserNormal() const { return spec_normal_; }
   const std::vector<float>& Texcoord() const { return texcoord_; }
@@ -997,18 +999,33 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
   // sets properties of a bounding volume given a face id
   void SetBoundingVolume(int faceid);
 
-  void LoadOBJ(mjResource* resource);  // load mesh in wavefront OBJ format
+  // load from OBJ, STL, or MSH file; throws mjCError on failure
-  void LoadSTL(mjResource* resource);  // load mesh in STL BIN format
+  void LoadFromResource(mjResource* resource, bool remove_repeated = false);
-  void LoadMSH(mjResource* resource);  // load mesh in MSH BIN format
 
-  void RemoveRepeated();               // remove repeated vertices
+  static bool IsObj(std::string_view filename, std::string_view ct = "");
+  static bool IsSTL(std::string_view filename, std::string_view ct = "");
+  static bool IsMSH(std::string_view filename, std::string_view ct = "");
+
+  bool IsObj() const;
+  bool IsSTL() const;
+  bool IsMSH() const;
 
  private:
+  void TryCompile(const mjVFS* vfs);
+
   // load mesh from cache asset, return true on success (OBJ files are only supported)
   bool LoadCachedMesh(mjCCache *cache, const mjResource* resource);
+
   // store mesh into asset cache (OBJ files are only supported)
-  void CacheMesh(mjCCache *cache, const mjResource* resource,
+  void CacheMesh(mjCCache *cache, const mjResource* resource);
-                 std::string_view asset_type);
+
+  // convert vertices to double precision and remove repeated vertices if requested
+  void ProcessVertices(const std::vector<float>& vert, bool remove_repeated = false);
+
+
+  void LoadOBJ(mjResource* resource, bool remove_repeated);  // load mesh in wavefront OBJ format
+  void LoadSTL(mjResource* resource);                        // load mesh in STL BIN format
+  void LoadMSH(mjResource* resource, bool remove_repeated);  // load mesh in MSH BIN format
 
   void LoadSDF();                               // generate mesh using marching cubes
   void MakeGraph();                             // make graph of convex hull

src/user/user_util.cc

@@ -359,7 +359,8 @@ void mjuu_crossvec(double* a, const double* b, const double* c) {
 
 
 // compute normal vector to given triangle, return length
-double mjuu_makenormal(double* normal, const float* a, const float* b, const float* c) {
+template<typename T> double mjuu_makenormal(double* normal, const T a[3],
+                                            const T b[3], const T c[3]) {
   double v1[3] = {a[0], a[1], a[2]};
   double v2[3] = {b[0], b[1], b[2]};
   double v3[3] = {c[0], c[1], c[2]};
@@ -379,6 +380,10 @@ double mjuu_makenormal(double* normal, const float* a, const float* b, const flo
   return nrm;
 }
 
+template double mjuu_makenormal(double* normal, const double a[3],
+                                const double b[3], const double c[3]);
+template double mjuu_makenormal(double* normal, const float a[3],
+                                const float b[3], const float c[3]);
 
 // compute quaternion as minimal rotation from [0;0;1] to vec
 void mjuu_z2quat(double* quat, const double* vec) {

src/user/user_util.h

@@ -107,8 +107,9 @@ void mjuu_localquat(double* local, const double* child, const double* parent);
 // compute vector cross-product a = b x c
 void mjuu_crossvec(double* a, const double* b, const double* c);
 
-// compute normal vector to given triangle (uses float for OpenGL)
+// compute normal vector to given triangle
-double mjuu_makenormal(double* normal, const float* a, const float* b, const float* c);
+template<typename T> double mjuu_makenormal(double* normal, const T a[3],
+                                            const T b[3], const T c[3]);
 
 // compute quaternion corresponding to minimal rotation from [0;0;1] to vec
 void mjuu_z2quat(double* quat, const double* vec);

test/engine/engine_collision_gjk_test.cc

@@ -991,7 +991,7 @@ TEST_F(MjGjkTest, SmallBoxMesh) {
   EXPECT_NEAR(dir[2], 1, kTolerance);
 
   // position
-  EXPECT_NEAR(mju_abs(pos[0]), 0.08333333, kTolerance);  // -pos[0] on ARM
+  EXPECT_NEAR(pos[0], 0, kTolerance);
   EXPECT_NEAR(pos[1], 0, kTolerance);
   EXPECT_NEAR(pos[2], 0, kTolerance);
 

test/user/user_mesh_test.cc

@@ -85,6 +85,7 @@ TEST_F(MjCMeshTest, UnknownMeshFormat) {
       </worldbody>
     </mujoco>
   )";
+
   std::vector<std::string> invalid_names = {
     "noextension",
     "anobj",
@@ -92,16 +93,22 @@ TEST_F(MjCMeshTest, UnknownMeshFormat) {
     "mesh.exe",
     "file%s"
   };
+  mjVFS vfs;
+  mj_defaultVFS(&vfs);
+
   for (const auto& name : invalid_names) {
+    mj_addBufferVFS(&vfs, name.c_str(), nullptr, 0);
     std::string xml = absl::StrFormat(xml_format, name);
     std::array<char, 1024> error;
     mjModel* model =
-        LoadModelFromString(xml.c_str(), error.data(), error.size());
+        LoadModelFromString(xml.c_str(), error.data(), error.size(), &vfs);
     ASSERT_THAT(model, testing::IsNull())
         << "Should fail to load a mesh named: " << name;
-    EXPECT_THAT(error.data(), HasSubstr("unknown mesh content type for file"));
+    EXPECT_THAT(error.data(), HasSubstr("unknown or unsupported mesh file: "));
     EXPECT_THAT(error.data(), HasSubstr(name));
   }
+
+  mj_deleteVFS(&vfs);
 }
 
 // -------------------- test OS filesystem fallback ----------------------------
@@ -280,14 +287,42 @@ TEST_F(MjCMeshTest, LoadMSHWithContentTypeError) {
   size_t error_sz = 1024;
 
   // load VFS on the heap
-  auto vfs = std::make_unique<mjVFS>();
+  mjVFS vfs;
-  mj_defaultVFS(vfs.get());
+  mj_defaultVFS(&vfs);
+  mj_addBufferVFS(&vfs, "some_file", nullptr, 0);
 
   // should error with unknown file type
-  mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
+  mjModel* model = LoadModelFromString(xml, error, error_sz, &vfs);
   EXPECT_THAT(model, IsNull());
-  EXPECT_THAT(error, HasSubstr("unsupported content type: 'model/unknown'"));
+  EXPECT_THAT(error, HasSubstr("unsupported mesh type: 'model/unknown'"));
-  mj_deleteVFS(vfs.get());
+  mj_deleteVFS(&vfs);
+}
+
+TEST_F(MjCMeshTest, LoadMSHWithInvalidContentType) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="mesh1" content_type="model" file="some_file"/>
+    </asset>
+    <worldbody>
+      <geom type="mesh" mesh="mesh1"/>
+    </worldbody>
+  </mujoco>
+  )";
+
+  char error[1024];
+  size_t error_sz = 1024;
+
+  // load VFS on the heap
+  mjVFS vfs;
+  mj_defaultVFS(&vfs);
+  mj_addBufferVFS(&vfs, "some_file", nullptr, 0);
+
+  // should error with unknown file type
+  mjModel* model = LoadModelFromString(xml, error, error_sz, &vfs);
+  EXPECT_THAT(model, IsNull());
+  EXPECT_THAT(error, HasSubstr("invalid content type: 'model'"));
+  mj_deleteVFS(&vfs);
 }
 
 TEST_F(MjCMeshTest, LoadMSHWithContentTypeParam) {
@@ -323,6 +358,7 @@ TEST_F(MjCMeshTest, DeduplicateSTLVertices) {
   char error[1024];
   size_t error_sz = 1024;
   mjModel* model = mj_loadXML(xml_path.c_str(), 0, error, error_sz);
+  ASSERT_THAT(model, NotNull()) << error;
   ASSERT_EQ(model->nmeshvert, 4);
   mj_deleteModel(model);
 }
@@ -519,9 +555,9 @@ TEST_F(MjCMeshTest, MissingFaceAllowedConvexInertia) {
   mjModel* model = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
   ASSERT_THAT(model, NotNull()) << error;
   EXPECT_THAT(model->nmeshface, 10);
-  EXPECT_THAT(model->body_inertia[3], model->body_inertia[9]);
+  EXPECT_NE(model->body_inertia[3], model->body_inertia[9]);
-  EXPECT_THAT(model->body_inertia[4], model->body_inertia[10]);
+  EXPECT_NE(model->body_inertia[4], model->body_inertia[10]);
-  EXPECT_THAT(model->body_inertia[5], model->body_inertia[11]);
+  EXPECT_NE(model->body_inertia[5], model->body_inertia[11]);
   EXPECT_NE(model->body_inertia[3], model->body_inertia[6]);
   EXPECT_NE(model->body_inertia[4], model->body_inertia[7]);
   EXPECT_NE(model->body_inertia[5], model->body_inertia[8]);