From 4d024f94214a8fea1f803a38697c7be8106adb33 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Fri, 7 Mar 2025 05:16:54 -0800 Subject: [PATCH] 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 | 15 +- src/user/user_flexcomp.cc | 49 +- src/user/user_mesh.cc | 621 ++++++++++++----------- src/user/user_objects.cc | 2 +- src/user/user_objects.h | 41 +- src/user/user_util.cc | 7 +- src/user/user_util.h | 5 +- test/engine/engine_collision_gjk_test.cc | 2 +- test/user/user_mesh_test.cc | 56 +- 9 files changed, 435 insertions(+), 363 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index f733d08c..a3df8d0e 100644 --- a/doc/XMLreference.rst +++ b/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 - to populate the point and element data of the flexcomp. :at:`dim` is automatically set to 2. Recall that a mesh asset - in MuJoCo can be used as a rigid geom attached to a single body. In 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). + assets, excluding the legacy .msh format. A mesh asset is not actually added to the model. Instead the vertex and + face data from the mesh file are used to populate the point and element data of the flexcomp. :at:`dim` is + automatically set to 2. Recall that a mesh asset in MuJoCo can be used as a rigid geom attached to a single body. In + 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 - :ref:`mesh assets` and also including GMSH. Volumetric meshes are supported only in GMSH format. - See :ref:`here` for more information on GMSH files. + surface meshes, the file extension is used to determine the file format. Supported formats are GMSH and the formats + specified in :ref:`mesh assets`, excluding the legacy .msh format. Volumetric meshes are supported only + in GMSH format. See :ref:`here` for more information on GMSH files. .. _body-flexcomp-rigid: diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index b17c2ca7..a900abc3 100644 --- a/src/user/user_flexcomp.cc +++ b/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.LoadSTL(resource); - } else if (!strcasecmp(ext.c_str(), ".obj")) { - isobj = true; - mesh.LoadOBJ(resource); - } else { - mesh.LoadMSH(resource); - } + mesh.LoadFromResource(resource, true); mju_closeResource(resource); } catch (mjCError err) { mju_closeResource(resource); return comperr(error, err.message, error_sz); } - // LoadOBJ uses userXXX, extra processing needed - if (isobj) { - // check sizes - 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(); + // check sizes + if (mesh.Vert().empty() || mesh.Face().empty()) { + return comperr(error, "Vertex and face data required", error_sz); } - // copy vertices, convert from float to double - point = vector (mesh.nvert()*3); - for (int i=0; i < mesh.nvert()*3; i++) { - point[i] = (double) mesh.Vert(i); - } + // copy vertices + point = mesh.Vert(); if (mesh.HasTexcoord()) { texcoord = mesh.Texcoord(); diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index e86c60bc..32b281de 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -76,8 +76,8 @@ namespace { } // namespace // compute triangle area, surface normal, center -static double _triangle(double* normal, double* center, - const float* v1, const float* v2, const float* v3) { +static double triangle(double* normal, double* center, + const double* v1, const double* v2, const double* v3) { // center if (center) { for (int i=0; i < 3; i++) { @@ -94,7 +94,7 @@ static double _triangle(double* normal, double* center, double len = sqrt(mjuu_dot3(normal, normal)); // ignore small faces - if (len(this) = spec; file_ = spec_file_; content_type_ = spec_content_type_; - vert_ = spec_vert_; normal_ = spec_normal_; face_ = spec_face_; + ProcessVertices(spec_vert_); texcoord_ = spec_texcoord_; facetexcoord_ = spec_facetexcoord_; maxhullvert_ = spec.maxhullvert; @@ -339,32 +339,35 @@ void mjCMesh::LoadSDF() { std::vector usernormal; std::vector userface; - for (size_t i = 0; i < mesh.vertices.size(); i++) { - uservert.push_back(2*aabb[3]*mesh.vertices.at(i).x/(nx-1) + aabb[0]-aabb[3]); - uservert.push_back(2*aabb[4]*mesh.vertices.at(i).y/(ny-1) + aabb[1]-aabb[4]); - uservert.push_back(2*aabb[5]*mesh.vertices.at(i).z/(nz-1) + aabb[2]-aabb[5]); + uservert.reserve(mesh.vertices.size() * 3); + usernormal.reserve(mesh.normals.size() * 3); + userface.reserve(mesh.indices.size()); + + for (const auto& vertex : mesh.vertices) { + uservert.push_back(2*aabb[3]*vertex.x/(nx-1) + aabb[0]-aabb[3]); + uservert.push_back(2*aabb[4]*vertex.y/(ny-1) + aabb[1]-aabb[4]); + uservert.push_back(2*aabb[5]*vertex.z/(nz-1) + aabb[2]-aabb[5]); } - for (size_t i = 0; i < mesh.normals.size(); i++) { - usernormal.push_back(mesh.normals.at(i).x); - usernormal.push_back(mesh.normals.at(i).y); - usernormal.push_back(mesh.normals.at(i).z); + for (const auto& normal : mesh.normals) { + usernormal.push_back(normal.x); + usernormal.push_back(normal.y); + usernormal.push_back(normal.z); } - for (size_t i = 0; i < mesh.indices.size(); i++) { - userface.push_back(mesh.indices.at(i)); + for (unsigned int index : mesh.indices) { + userface.push_back(index); } - vert_ = std::move(uservert); normal_ = std::move(usernormal); face_ = std::move(userface); + ProcessVertices(uservert); delete[] field; } -void mjCMesh::CacheMesh(mjCCache* cache, const mjResource* resource, - std::string_view asset_type) { +void mjCMesh::CacheMesh(mjCCache* cache, const mjResource* resource) { if (cache == nullptr) return; - if (asset_type != "model/obj") return; // only OBJ files are cached + if (!IsObj()) return; // only OBJ files are cached // cache mesh data into new mesh object mjCMesh *mesh = new mjCMesh(); @@ -381,7 +384,7 @@ void mjCMesh::CacheMesh(mjCCache* cache, const mjResource* resource, // calculate estimated size of mesh std::size_t size = sizeof(mjCMesh) - + (sizeof(float) * vert_.size()) + + (sizeof(double) * vert_.size()) + (sizeof(float) * normal_.size()) + (sizeof(float) * texcoord_.size()) + (sizeof(int) * face_.size()) @@ -399,12 +402,168 @@ void mjCMesh::CacheMesh(mjCCache* cache, const mjResource* resource, cache->Insert("", resource, cached_data, size); } -// compiler +namespace { + +// vertex key for hash map +struct VertexKey { + float v[3]; + + bool operator==(const VertexKey &other) const { + return (v[0] == other.v[0] && v[1] == other.v[1] && v[2] == other.v[2]); + } + + std::size_t operator()(const VertexKey& vertex) const { + // combine all three hash values into a single hash value + return ((std::hash()(vertex.v[0]) + ^ (std::hash()(vertex.v[1]) << 1)) >> 1) + ^ (std::hash()(vertex.v[2]) << 1); + } +}; + +} // namespace + +// convert vertices to double precision and remove repeated vertices if requested +void mjCMesh::ProcessVertices(const std::vector& vert, bool remove_repeated) { + vert_.clear(); + int nvert = vert.size(); + + if (nvert % 3) { + throw mjCError(this, "vertex data must be a multiple of 3"); + } + if (face_.size() % 3) { + throw mjCError(this, "face data must be a multiple of 3"); + } + + // convert vertices to double precision, may contain repeated vertices + if (!remove_repeated) { + vert_.reserve(nvert); + for (int i = 0; i < nvert / 3; ++i) { + const float* v = &vert[3 * i]; + if (!std::isfinite(v[0]) || !std::isfinite(v[1]) || !std::isfinite(v[2])) { + throw mjCError(this, "vertex coordinate %d is not finite", nullptr, i); + } + vert_.push_back(v[0]); + vert_.push_back(v[1]); + vert_.push_back(v[2]); + } + return; + } + + int index = 0; + std::unordered_map vertex_map; + + // populate vertex map with new vertex indices + for (int i = 0; i < nvert; i += 3) { + const float* v = &vert[i]; + + if (!std::isfinite(v[0]) || !std::isfinite(v[1]) || !std::isfinite(v[2])) { + throw mjCError(this, "vertex coordinate %d is not finite", nullptr, i); + } + + VertexKey key = {v[0], v[1], v[2]}; + if (vertex_map.find(key) == vertex_map.end()) { + vertex_map.insert({key, index}); + ++index; + } + } + + // no repeated vertices (just copy vertex data) + if (3*index == nvert) { + vert_.reserve(nvert); + for (float v : vert) { + vert_.push_back(v); + } + return; + } + + // update face vertex indices + for (int i = 0; i < face_.size(); ++i) { + VertexKey key = {vert[3*face_[i]], vert[3*face_[i] + 1], + vert[3*face_[i] + 2]}; + face_[i] = vertex_map[key]; + } + + // repopulate vertex data + vert_.resize(3 * index); + for (const auto& pair : vertex_map) { + const VertexKey& key = pair.first; + int index = pair.second; + + // double precision + vert_[3*index + 0] = key.v[0]; + vert_[3*index + 1] = key.v[1]; + vert_[3*index + 2] = key.v[2]; + } +} + +bool mjCMesh::IsObj(std::string_view filename, std::string_view ct) { + std::string asset_type = GetAssetContentType(filename, ct); + return asset_type == "model/obj"; +} + +bool mjCMesh::IsSTL(std::string_view filename, std::string_view ct) { + std::string asset_type = GetAssetContentType(filename, ct); + return asset_type == "model/stl"; +} + +bool mjCMesh::IsMSH(std::string_view filename, std::string_view ct) { + std::string asset_type = GetAssetContentType(filename, ct); + return asset_type == "model/vnd.mujoco.msh"; +} + +bool mjCMesh::IsObj() const{ + return content_type_ == "model/obj"; +} + +bool mjCMesh::IsSTL() const { + return content_type_ == "model/stl"; +} + +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 + std::string asset_type = GetAssetContentType(resource->name, content_type_); + if (asset_type.empty()) { + if (!content_type_.empty()) { + throw mjCError(this, "invalid content type: '%s'", content_type_.c_str()); + } + throw mjCError(this, "unknown or unsupported mesh file: '%s'", resource->name); + } + content_type_ = asset_type; + + if (IsSTL()) { + LoadSTL(resource); + } else if (IsObj()) { + LoadOBJ(resource, remove_repeated); + } else if (IsMSH()) { + LoadMSH(resource, remove_repeated); + } else { + throw mjCError(this, "unsupported mesh type: '%s'", asset_type.c_str()); + } +} + +// compiler wrapper void mjCMesh::Compile(const mjVFS* vfs) { + try { + TryCompile(vfs); + } catch (mjCError err) { + if (resource_ != nullptr) { + mju_closeResource(resource_); + resource_ = nullptr; + } + throw err; + } +} + +// compiler +void mjCMesh::TryCompile(const mjVFS* vfs) { + bool fromCache = false; CopyFromSpec(); visual_ = true; - std::string asset_type = GetAssetContentType(file_, content_type_); - mjResource* resource = nullptr; mjCCache *cache = reinterpret_cast(mj_globalCache()); // load file @@ -415,6 +574,10 @@ void mjCMesh::Compile(const mjVFS* vfs) { texcoord_.clear(); facenormal_.clear(); facetexcoord_.clear(); + if (resource_ != nullptr) { + mju_closeResource(resource_); + resource_ = nullptr; + } // copy paths from model if not already defined if (modelfiledir_.empty()) { @@ -429,48 +592,22 @@ void mjCMesh::Compile(const mjVFS* vfs) { file_ = mjuu_strippath(file_); } - if (asset_type.empty()) { - throw mjCError(this, "unknown mesh content type for file: '%s'", file_.c_str()); - } - - if (asset_type != "model/stl" && asset_type != "model/obj" - && asset_type != "model/vnd.mujoco.msh") { - throw mjCError(this, "unsupported content type: '%s'", asset_type.c_str()); - } - FilePath filename = meshdir_ + FilePath(file_); - resource = LoadResource(modelfiledir_.Str(), filename.Str(), vfs); + resource_ = LoadResource(modelfiledir_.Str(), filename.Str(), vfs); // try loading from cache - if (cache != nullptr && LoadCachedMesh(cache, resource)) { - mju_closeResource(resource); - resource = nullptr; + if (cache != nullptr && LoadCachedMesh(cache, resource_)) { + mju_closeResource(resource_); + resource_ = nullptr; + fromCache = true; } - if (resource != nullptr) { - try { - if (asset_type == "model/stl") { - LoadSTL(resource); - } else if (asset_type == "model/obj"){ - LoadOBJ(resource); - } else { - LoadMSH(resource); - } - } catch (mjCError err) { - mju_closeResource(resource); - throw err; - } - - CacheMesh(cache, resource, asset_type); - mju_closeResource(resource); + if (!fromCache) { + LoadFromResource(resource_); + CacheMesh(cache, resource_); } // check repeated mesh data - if (!vert_.empty() && !spec_vert_.empty()) { - throw mjCError(this, "repeated vertex specification"); - } else if (vert_.empty()) { - vert_ = spec_vert_; - } if (!normal_.empty() && !spec_normal_.empty()) { throw mjCError(this, "repeated normal specification"); } else if (normal_.empty()) { @@ -486,6 +623,11 @@ void mjCMesh::Compile(const mjVFS* vfs) { } else if (face_.empty()) { face_ = spec_face_; } + if (!vert_.empty() && !spec_vert_.empty()) { + throw mjCError(this, "repeated vertex specification"); + } else if (vert_.empty()) { + ProcessVertices(spec_vert_); + } if (!facenormal_.empty() && !spec_normal_.empty()) { throw mjCError(this, "repeated facenormal specification"); } else if (facenormal_.empty()) { @@ -509,11 +651,16 @@ void mjCMesh::Compile(const mjVFS* vfs) { // check texcoord size if no face texcoord indices are given if (!texcoord_.empty() && texcoord_.size() != 2 * nvert() && - facetexcoord_.empty() && asset_type != "model/obj") { + facetexcoord_.empty() && !IsObj()) { throw mjCError(this, "texcoord must be 2*nv if face texcoord indices are not provided in an OBJ file"); } + // require vertices + if (vert_.empty()) { + throw mjCError(this, "no vertices"); + } + // check vertices exist for (int i=0; i < face_.size(); i++) { if (face_[i] >= nvert() || face_[i] < 0) { @@ -529,8 +676,8 @@ void mjCMesh::Compile(const mjVFS* vfs) { int v1 = face_[3*i+1]; int v2 = face_[3*i+2]; double normal[3]; - float* vtx = vert_.data(); - if (_triangle(normal, nullptr, vtx+3*v0, vtx+3*v1, vtx+3*v2)>sqrt(mjMINVAL)) { + double* vtx = vert_.data(); + if (triangle(normal, nullptr, vtx+3*v0, vtx+3*v1, vtx+3*v2)>sqrt(mjMINVAL)) { halfedge_.push_back(std::pair(v0, v1)); halfedge_.push_back(std::pair(v1, v2)); halfedge_.push_back(std::pair(v2, v0)); @@ -540,9 +687,9 @@ void mjCMesh::Compile(const mjVFS* vfs) { } } - // check vertices exist - for (auto vertex_index : face_) { - if (vertex_index>=nvert() || vertex_index < 0) { + // check that vertex indices are valid + for (int vertex_index : face_) { + if (vertex_index >= nvert() || vertex_index < 0) { throw mjCError(this, "found index in userface that exceeds uservert size."); } } @@ -557,11 +704,6 @@ void mjCMesh::Compile(const mjVFS* vfs) { } } - // require vertices - if (vert_.empty()) { - throw mjCError(this, "no vertices"); - } - // make graph describing convex hull if (needhull_ || face_.empty()) { MakeGraph(); @@ -619,6 +761,9 @@ void mjCMesh::Compile(const mjVFS* vfs) { // check that processed mesh is valid CheckMesh(); + + mju_closeResource(resource_); + resource_ = nullptr; } @@ -671,7 +816,9 @@ bool mjCMesh::HasTexcoord() const { void mjCMesh::CopyVert(float* arr) const { - std::copy(vert_.begin(), vert_.end(), arr); + for (int i = 0; i < vert_.size(); ++i) { + arr[i] = (float)vert_[i]; + } } @@ -867,89 +1014,8 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) { -// comparison function for vertex sorting -bool vertcompare(int index1, int index2, const std::vector& vert) { - for (int i = 0; i < 3; i++) { - if (vert[3*index1 + i] < vert[3*index2 + i]) { - return true; - } - if (vert[3*index1 + i] > vert[3*index2 + i]) { - return false; - } - } - return false; -} - -// remove repeated vertices -void mjCMesh::RemoveRepeated() { - int repeated = 0; - - std::vector index(nvert()); - std::vector redirect(nvert()); - for (int i=0; i < nvert(); i++) { - index[i] = redirect[i] = i; - } - - std::stable_sort(index.begin(), index.end(), [&vert = vert_](int a, int b) { - return vertcompare(a, b, vert); - }); - - // find repeated vertices, set redirect - for (int i=1; i < nvert(); i++) { - if (vert_[3*index[i]] == vert_[3*index[i-1]] && - vert_[3*index[i]+1] == vert_[3*index[i-1]+1] && - vert_[3*index[i]+2] == vert_[3*index[i-1]+2]) { - redirect[index[i]] = index[i-1]; - repeated++; - } - } - - // compress vertices, change face data - if (repeated) { - // track redirections until non-redirected vertex, set - for (int i=0; i < nvert(); i++) { - int j = i; - while (redirect[j]!=j) { - j = redirect[j]; - } - redirect[i] = j; - } - - // find good vertices, compress, reuse index to save compressed position - int j = 0; - for (int i=0; i < nvert(); i++) { - if (redirect[i]==i) { - index[i] = j; - memcpy(vert_.data()+3*j, vert_.data()+3*i, 3*sizeof(float)); - j++; - } else { - index[i] = -1; - } - } - - // recompute face data to reflect compressed vertices - for (int i=0; i < 3*nface(); i++) { - face_[i] = index[redirect[face_[i]]]; - - // sanity check, SHOULD NOT OCCUR - if (face_[i]<0 || face_[i]>=nvert()-repeated) { - throw mjCError( - this, "error removing vertices from mesh '%s'", name.c_str()); - } - } - } - - // resize vert if any vertices were removed - if (repeated) { - std::vector old = vert_; - vert_.assign(3*(nvert()-repeated), 0); - memcpy(vert_.data(), old.data(), 3*nvert()*sizeof(float)); - } -} - - // load OBJ mesh -void mjCMesh::LoadOBJ(mjResource* resource) { +void mjCMesh::LoadOBJ(mjResource* resource, bool remove_repeated) { tinyobj::ObjReader objReader; const void* bytes = nullptr; @@ -967,7 +1033,6 @@ void mjCMesh::LoadOBJ(mjResource* resource) { } const auto& attrib = objReader.GetAttrib(); - vert_ = attrib.vertices; // copy from one std::vector to another normal_ = attrib.normals; texcoord_ = attrib.texcoords; facenormal_.clear(); @@ -975,7 +1040,7 @@ void mjCMesh::LoadOBJ(mjResource* resource) { if (!objReader.GetShapes().empty()) { const auto& mesh = objReader.GetShapes()[0].mesh; - bool righthand = (scale[0]*scale[1]*scale[2] > 0); + bool righthand = scale[0] * scale[1] * scale[2] > 0; // iterate over mesh faces std::vector face_indices; @@ -988,13 +1053,13 @@ void mjCMesh::LoadOBJ(mjResource* resource) { } face_indices.push_back(mesh.indices[idx]); - face_indices.push_back(mesh.indices[idx + (righthand==1 ? 1 : 2)]); - face_indices.push_back(mesh.indices[idx + (righthand==1 ? 2 : 1)]); + face_indices.push_back(mesh.indices[idx + (righthand == 1 ? 1 : 2)]); + face_indices.push_back(mesh.indices[idx + (righthand == 1 ? 2 : 1)]); if (nfacevert == 4) { face_indices.push_back(mesh.indices[idx]); - face_indices.push_back(mesh.indices[idx + (righthand==1 ? 2 : 3)]); - face_indices.push_back(mesh.indices[idx + (righthand==1 ? 3 : 2)]); + face_indices.push_back(mesh.indices[idx + (righthand == 1 ? 2 : 3)]); + face_indices.push_back(mesh.indices[idx + (righthand == 1 ? 3 : 2)]); } idx += nfacevert; ++face; @@ -1034,6 +1099,9 @@ void mjCMesh::LoadOBJ(mjResource* resource) { texcoord_index_.push_back(index.texcoord_index); } } + + // copy vertex data + ProcessVertices(attrib.vertices, remove_repeated); } @@ -1056,7 +1124,6 @@ bool mjCMesh::LoadCachedMesh(mjCCache *cache, const mjResource* resource) { bool righthand = (scale[0] * scale[1] * scale[2]) > 0; - for (int face = 0, i = 0; i < vertex_index_.size();) { int nfacevert = num_face_vertices_[face]; if (nfacevert < 3 || nfacevert > 4) { @@ -1106,7 +1173,7 @@ bool mjCMesh::LoadCachedMesh(mjCCache *cache, const mjResource* resource) { // load STL binary mesh void mjCMesh::LoadSTL(mjResource* resource) { - bool righthand = (scale[0]*scale[1]*scale[2]>0); + bool righthand = scale[0] * scale[1] * scale[2] > 0; // get file data in buffer char* buffer = 0; @@ -1120,14 +1187,14 @@ void mjCMesh::LoadSTL(mjResource* resource) { } // make sure there is enough data for header - if (buffer_sz<84) { + if (buffer_sz < 84) { throw mjCError(this, "invalid header in STL file '%s'", resource->name); } // get number of triangles, check bounds int nfaces = 0; ReadFromBuffer(&nfaces, buffer + 80); - if (nfaces<1 || nfaces>200000) { + if (nfaces < 1 || nfaces > 200000) { throw mjCError(this, "number of faces should be between 1 and 200000 in STL file '%s';" " perhaps this is an ASCII file?", resource->name); @@ -1145,50 +1212,41 @@ void mjCMesh::LoadSTL(mjResource* resource) { // allocate face and vertex data face_.assign(3*nfaces, 0); - vert_.clear(); + std::vector vert; // add vertices and faces, including repeated for now for (int i=0; i < nfaces; i++) { - for (int j=0; j<3; j++) { + for (int j=0; j < 3; j++) { // read vertex coordinates float v[3]; - ReadFromBuffer(&v, stl+50*i+12*(j+1)); + ReadFromBuffer(&v, stl + 50*i + 12*(j + 1)); - for (int k=0; k < 3; k++) { - if (std::isnan(v[k]) || std::isinf(v[k])) { - throw mjCError(this, "STL file '%s' contains invalid vertices.", - resource->name); - } - // check if vertex coordinates can be cast to an int safely - if (fabs(v[k]) > pow(2, 30)) { - throw mjCError(this, - "vertex coordinates in STL file '%s' exceed maximum bounds", - resource->name); - } + // check if vertex can be cast to an int safely + if (fabs(v[0]) > pow(2, 30) || fabs(v[1]) > pow(2, 30) || fabs(v[2]) > pow(2, 30)) { + throw mjCError(this, "vertex in STL file '%s' exceed maximum bounds", resource->name); } // add vertex address in face; change order if scale makes it lefthanded - if (righthand || j==0) { - face_[3*i+j] = nvert(); + if (righthand || j == 0) { + face_[3*i + j] = vert.size() / 3; } else { - face_[3*i+3-j] = nvert(); + face_[3*i + 3 - j] = vert.size() / 3; } // add vertex data - vert_.push_back(v[0]); - vert_.push_back(v[1]); - vert_.push_back(v[2]); + vert.push_back(v[0]); + vert.push_back(v[1]); + vert.push_back(v[2]); } } - - RemoveRepeated(); + ProcessVertices(vert, true); } // load MSH binary mesh -void mjCMesh::LoadMSH(mjResource* resource) { - bool righthand = (scale[0]*scale[1]*scale[2]>0); +void mjCMesh::LoadMSH(mjResource* resource, bool remove_repeated) { + bool righthand = scale[0] * scale[1] * scale[2] > 0; // get file data in buffer char* buffer = 0; @@ -1214,9 +1272,9 @@ void mjCMesh::LoadMSH(mjResource* resource) { ReadFromBuffer(&nfbuf, buffer + 3*sizeof(int)); // check sizes - if (nvbuf<4 || nfbuf<0 || nnbuf<0 || ntbuf<0 || - (nnbuf>0 && nnbuf!=nvbuf) || - (ntbuf>0 && ntbuf!=nvbuf)) { + if (nvbuf < 4 || nfbuf < 0 || nnbuf < 0 || ntbuf < 0 || + (nnbuf > 0 && nnbuf != nvbuf) || + (ntbuf > 0 && ntbuf != nvbuf)) { throw mjCError(this, "invalid sizes in MSH file '%s'", resource->name); } @@ -1235,20 +1293,23 @@ void mjCMesh::LoadMSH(mjResource* resource) { // allocate and copy using UnalignedFloat = char[sizeof(float)]; auto fdata = reinterpret_cast(buffer + 4*sizeof(int)); + std::vector vert; + int nvert = 0; if (nvbuf) { - vert_.assign(3*nvbuf, 0); - memcpy(vert_.data(), fdata, 3*nvert()*sizeof(float)); - fdata += 3*nvert(); + vert.assign(3*nvbuf, 0); + nvert = 3*nvbuf; + memcpy(vert.data(), fdata, nvert*sizeof(float)); + fdata += nvert; } if (nnbuf) { - normal_.assign(3*nvert(), 0); - memcpy(normal_.data(), fdata, 3*nvert()*sizeof(float)); - fdata += 3*nvert(); + normal_.assign(nvert, 0); + memcpy(normal_.data(), fdata, nvert*sizeof(float)); + fdata += nvert; } if (ntbuf) { - texcoord_.assign(2*nvert(), 0); - memcpy(texcoord_.data(), fdata, 2*nvert()*sizeof(float)); - fdata += 2*nvert(); + texcoord_.assign(2*(nvert / 3), 0); + memcpy(texcoord_.data(), fdata, 2*(nvert/3)*sizeof(float)); + fdata += 2*(nvert / 3); } if (nfbuf) { face_.assign(3*nfbuf, 0); @@ -1269,6 +1330,7 @@ void mjCMesh::LoadMSH(mjResource* resource) { face_[3*i+2] = tmp; } } + ProcessVertices(vert, remove_repeated); } @@ -1279,10 +1341,10 @@ void mjCMesh::ComputeVolume(double CoM[3], const double facecen[3]) { mjuu_zerovec(CoM, 3); int nf = (inertia == mjMESH_INERTIA_CONVEX) ? graph_[1] : nface(); int* f = (inertia == mjMESH_INERTIA_CONVEX) ? graph_ + 2 + 3*(graph_[0]+graph_[1]) : face_.data(); - float* vv = vert_.data(); + double* vv = vert_.data(); for (int i=0; i < nf; i++) { // get area, normal and center - double a = _triangle(nrm, cen, vv+3*f[3*i], vv+3*f[3*i+1], vv+3*f[3*i+2]); + double a = triangle(nrm, cen, vv+3*f[3*i], vv+3*f[3*i+1], vv+3*f[3*i+2]); // compute and add volume const double vec[3] = {cen[0]-facecen[0], cen[1]-facecen[1], cen[2]-facecen[2]}; @@ -1316,11 +1378,11 @@ void mjCMesh::ComputeSurfaceArea(double CoM[3], const double facecen[3]) { double cen[3]; GetVolumeRef() = 0; mjuu_zerovec(CoM, 3); - float* vv = vert_.data(); + double* vv = vert_.data(); for (int i=0; i < nface(); i++) { // get area, normal and center - double a = _triangle(nrm, cen, vv+3*face_.data()[3*i], - vv+3*face_.data()[3*i+1], vv+3*face_.data()[3*i+2]); + double a = triangle(nrm, cen, vv+3*face_.data()[3*i], + vv+3*face_.data()[3*i+1], vv+3*face_.data()[3*i+2]); // add pyramid com GetVolumeRef() += a; @@ -1343,20 +1405,16 @@ void mjCMesh::ComputeSurfaceArea(double CoM[3], const double facecen[3]) { // apply transformations void mjCMesh::ApplyTransformations() { // translate - if (refpos[0]!=0 || refpos[1]!=0 || refpos[2]!=0) { - // prepare translation - float rp[3] = {(float)refpos[0], (float)refpos[1], (float)refpos[2]}; - - // process vertices - for (int i=0; i < nvert(); i++) { - vert_[3*i] -= rp[0]; - vert_[3*i+1] -= rp[1]; - vert_[3*i+2] -= rp[2]; + if (refpos[0] != 0 || refpos[1] != 0 || refpos[2] != 0) { + for (int i = 0; i < nvert(); i++) { + vert_[3*i + 0] -= refpos[0]; + vert_[3*i + 1] -= refpos[1]; + vert_[3*i + 2] -= refpos[2]; } } // rotate - if (refquat[0]!=1 || refquat[1]!=0 || refquat[2]!=0 || refquat[3]!=0) { + if (refquat[0] != 1 || refquat[1] != 0 || refquat[2] != 0 || refquat[3] != 0) { // prepare rotation double quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]}; double mat[9]; @@ -1364,16 +1422,12 @@ void mjCMesh::ApplyTransformations() { mjuu_quat2mat(mat, quat); // process vertices - for (int i=0; i < nvert(); i++) { - double p1[3], p0[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; - mjuu_mulvecmatT(p1, p0, mat); - vert_[3*i] = (float) p1[0]; - vert_[3*i+1] = (float) p1[1]; - vert_[3*i+2] = (float) p1[2]; + for (int i = 0; i < nvert(); i++) { + mjuu_mulvecmatT(&vert_[3*i], &vert_[3*i], mat); } // process normals - for (int i=0; i < nnormal(); i++) { + for (int i = 0; i < nnormal(); i++) { double n1[3], n0[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]}; mjuu_mulvecmatT(n1, n0, mat); normal_[3*i] = (float) n1[0]; @@ -1383,35 +1437,35 @@ void mjCMesh::ApplyTransformations() { } // scale - if (scale[0]!=1 || scale[1]!=1 || scale[2]!=1) { - for (int i=0; i < nvert(); i++) { - vert_[3*i] *= scale[0]; - vert_[3*i+1] *= scale[1]; - vert_[3*i+2] *= scale[2]; + if (scale[0] != 1 || scale[1] != 1 || scale[2] != 1) { + for (int i = 0; i < nvert(); i++) { + vert_[3*i + 0] *= scale[0]; + vert_[3*i + 1] *= scale[1]; + vert_[3*i + 2] *= scale[2]; } - for (int i=0; i < nnormal(); i++) { - normal_[3*i] *= scale[0]; - normal_[3*i+1] *= scale[1]; - normal_[3*i+2] *= scale[2]; + for (int i = 0; i < nnormal(); i++) { + normal_[3*i + 0] *= scale[0]; + normal_[3*i + 1] *= scale[1]; + normal_[3*i + 2] *= scale[2]; } } // normalize normals - for (int i=0; i < nnormal(); i++) { + for (int i = 0; i < nnormal(); i++) { // compute length float len = normal_[3*i]*normal_[3*i] + normal_[3*i+1]*normal_[3*i+1] + normal_[3*i+2]*normal_[3*i+2]; // rescale - if (len>mjMINVAL) { + if (len > mjMINVAL) { float scl = 1/sqrtf(len); - normal_[3*i] *= scl; - normal_[3*i+1] *= scl; - normal_[3*i+2] *= scl; + normal_[3*i + 0] *= scl; + normal_[3*i + 1] *= scl; + normal_[3*i + 2] *= scl; } else { - normal_[3*i] = 0; - normal_[3*i+1] = 0; - normal_[3*i+2] = 1; + normal_[3*i + 0] = 0; + normal_[3*i + 1] = 0; + normal_[3*i + 2] = 1; } } } @@ -1432,8 +1486,8 @@ void mjCMesh::ComputeFaceCentroid(double facecen[3]) { } // get area and center - float* vv = vert_.data(); - double a = _triangle(nrm, cen, vv+3*face_[3*i], vv+3*face_[3*i+1], vv+3*face_[3*i+2]); + double* vv = vert_.data(); + double a = triangle(nrm, cen, vv+3*face_[3*i], vv+3*face_[3*i+1], vv+3*face_[3*i+2]); // accumulate for (int j=0; j<3; j++) { @@ -1527,13 +1581,13 @@ void mjCMesh::ComputeInertia(double inert[6], double CoM[3]) { double density = model->def_map[classname]->Geom().density; // copy vertices to avoid modifying the original mesh - std::vector vert_centered(vert_); + std::vector vert_centered(vert_); // translate vertices to origin in order to compute inertia for (int i=0; i < nvert(); i++) { - for (int j=0; j<3; j++) { - vert_centered[3*i+j] -= CoM[j]; - } + vert_centered[3*i + 0] -= CoM[0]; + vert_centered[3*i + 1] -= CoM[1]; + vert_centered[3*i + 2] -= CoM[2]; } // accumulate products of inertia, recompute volume @@ -1543,13 +1597,13 @@ void mjCMesh::ComputeInertia(double inert[6], double CoM[3]) { int nf = (inertia == mjMESH_INERTIA_CONVEX) ? graph_[1] : nface(); int* f = (inertia == mjMESH_INERTIA_CONVEX) ? graph_ + 2 + 3*(graph_[0]+graph_[1]) : face_.data(); for (int i=0; i < nf; i++) { - float* D = vert_centered.data()+3*f[3*i]; - float* E = vert_centered.data()+3*f[3*i+1]; - float* F = vert_centered.data()+3*f[3*i+2]; + double* D = &vert_centered[3*f[3*i + 0]]; + double* E = &vert_centered[3*f[3*i + 1]]; + double* F = &vert_centered[3*f[3*i + 2]]; // get area, normal and center; update volume - double a = _triangle(nrm, cen, D, E, F); - double vol = inertia==mjMESH_INERTIA_SHELL ? a : mjuu_dot3(cen, nrm) * a / 3; + double a = triangle(nrm, cen, D, E, F); + double vol = (inertia == mjMESH_INERTIA_SHELL) ? a : mjuu_dot3(cen, nrm) * a / 3; // if legacy computation requested, then always positive if (inertia == mjMESH_INERTIA_LEGACY) { @@ -1560,7 +1614,7 @@ void mjCMesh::ComputeInertia(double inert[6], double CoM[3]) { GetVolumeRef() += vol; for (int j=0; j<6; j++) { P[j] += density*vol / - (inertia==mjMESH_INERTIA_SHELL ? 12 : 20) * ( + (inertia == mjMESH_INERTIA_SHELL ? 12 : 20) * ( 2*(D[k[j][0]] * D[k[j][1]] + E[k[j][0]] * E[k[j][1]] + F[k[j][0]] * F[k[j][1]]) + @@ -1585,19 +1639,18 @@ void mjCMesh::Rotate(double quat[4]) { double neg[4] = {quat[0], -quat[1], -quat[2], -quat[3]}; double mat[9]; mjuu_quat2mat(mat, neg); - for (int i=0; i < nvert(); i++) { - // vertices - const double vec[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; - double res[3]; - mjuu_mulvecmat(res, vec, mat); - for (int j=0; j<3; j++) { - vert_[3*i+j] = (float) res[j]; + for (int i = 0; i < nvert(); i++) { + mjuu_mulvecmat(&vert_[3*i], &vert_[3*i], mat); - // axis-aligned bounding box - aamm_[j+0] = min(aamm_[j+0], res[j]); - aamm_[j+3] = max(aamm_[j+3], res[j]); - } + // axis-aligned bounding box + aamm_[0] = std::min(aamm_[0], vert_[3*i + 0]); + aamm_[3] = std::max(aamm_[3], vert_[3*i + 0]); + aamm_[1] = std::min(aamm_[1], vert_[3*i + 1]); + aamm_[4] = std::max(aamm_[4], vert_[3*i + 1]); + aamm_[2] = std::min(aamm_[2], vert_[3*i + 2]); + aamm_[5] = std::max(aamm_[5], vert_[3*i + 2]); } + for (int i=0; i < nnormal(); i++) { // normals const double nrm[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]}; @@ -1613,10 +1666,10 @@ void mjCMesh::Rotate(double quat[4]) { void mjCMesh::Transform(double pos[3], double quat[4]) { // subtract CoM position from vertices for (int i=0; i < nvert(); i++) { - for (int j=0; j<3; j++) { - vert_[3*i+j] -= pos[j]; - } - } + vert_[3*i + 0] -= pos[0]; + vert_[3*i + 1] -= pos[1]; + vert_[3*i + 2] -= pos[2]; + } Rotate(quat); // save the pos and quat that was used to transform the mesh @@ -1655,14 +1708,13 @@ double* mjCMesh::GetInertiaBoxPtr() { double& mjCMesh::GetVolumeRef() { CheckMesh(); - return inertia==mjMESH_INERTIA_SHELL ? surface_ : volume_; + return (inertia == mjMESH_INERTIA_SHELL) ? surface_ : volume_; } // make graph describing convex hull void mjCMesh::MakeGraph() { int adr, ok, curlong, totlong, exitcode; - double* data; facetT* facet, **facetp; vertexT* vertex, *vertex1, **vertex1p; @@ -1677,19 +1729,6 @@ void mjCMesh::MakeGraph() { return; } - // convert mesh data to double - data = (double*) mju_malloc(3*nvert()*sizeof(double)); - if (!data) { - throw mjCError(this, "could not allocate data for qhull"); - } - for (int i=0; i < 3*nvert(); i++) { - if (!std::isfinite(vert_[i])) { - mju_free(data); - throw mjCError(this, "vertex coordinate %d is not finite", NULL, i); - } - data[i] = (double)vert_[i]; - } - qhT qh_qh; qhT* qh = &qh_qh; qh_zero(qh, stderr); @@ -1703,7 +1742,7 @@ void mjCMesh::MakeGraph() { if (!exitcode) { // actual init qh_initflags(qh, const_cast(qhopt.c_str())); - qh_init_B(qh, data, nvert(), 3, False); + qh_init_B(qh, vert_.data(), nvert(), 3, False); // construct convex hull qh_qhull(qh); @@ -1814,7 +1853,6 @@ void mjCMesh::MakeGraph() { // free all qh_freeqhull(qh, !qh_ALL); qh_memfreeshort(qh, &curlong, &totlong); - mju_free(data); // bad graph: delete if (!ok) { @@ -1849,7 +1887,6 @@ void mjCMesh::MakeGraph() { // free all qh_freeqhull(qh, !qh_ALL); qh_memfreeshort(qh, &curlong, &totlong); - mju_free(data); if (graph_) { mju_free(graph_); szgraph_ = 0; @@ -1860,7 +1897,7 @@ void mjCMesh::MakeGraph() { } // copy graph into face data -void mjCMesh::CopyGraph(void) { +void mjCMesh::CopyGraph() { // only if face data is missing if (!face_.empty()) { return; @@ -1876,16 +1913,16 @@ void mjCMesh::CopyGraph(void) { int j = 2 + 3*numvert + 3*nface() + 3*i; // copy - face_[3*i] = graph_[j]; - face_[3*i+1] = graph_[j+1]; - face_[3*i+2] = graph_[j+2]; + face_[3*i + 0] = graph_[j + 0]; + face_[3*i + 1] = graph_[j + 1]; + face_[3*i + 2] = graph_[j + 2]; } } // compute vertex normals -void mjCMesh::MakeNormal(void) { +void mjCMesh::MakeNormal() { // only if normal data is missing if (!normal_.empty()) { return; @@ -1996,7 +2033,7 @@ void mjCMesh::MakeNormal(void) { // compute face circumradii -void mjCMesh::MakeCenter(void) { +void mjCMesh::MakeCenter() { if (center_) { return; } @@ -2058,7 +2095,7 @@ void mjCMesh::MakePolygonNormals() { class MeshPolygon { public: // constructors (need starting face) - MeshPolygon(const float v1[3], const float v2[3], const float v3[3], + MeshPolygon(const double v1[3], const double v2[3], const double v3[3], int v1i, int v2i, int v3i); MeshPolygon() = delete; MeshPolygon(const MeshPolygon&) = delete; @@ -2084,7 +2121,7 @@ class MeshPolygon { -MeshPolygon::MeshPolygon(const float v1[3], const float v2[3], const float v3[3], +MeshPolygon::MeshPolygon(const double v1[3], const double v2[3], const double v3[3], int v1i, int v2i, int v3i) { mjuu_makenormal(normal_, v1, v2, v3); edges_ = {{v1i, v2i}, {v2i, v3i}, {v3i, v1i}}; @@ -2293,9 +2330,9 @@ void mjCMesh::MakePolygons() { // process each face for (int i = 0; i < nfaces; i++) { - float* v1 = &vert_[3*faces[3*i + 0]]; - float* v2 = &vert_[3*faces[3*i + 1]]; - float* v3 = &vert_[3*faces[3*i + 2]]; + double* v1 = &vert_[3*faces[3*i + 0]]; + double* v2 = &vert_[3*faces[3*i + 1]]; + double* v3 = &vert_[3*faces[3*i + 2]]; MeshPolygon face(v1, v2, v3, faces[3*i + 0], faces[3*i + 1], faces[3*i + 2]); auto it = polygons.find(face); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 102e9c4c..42194aee 100644 --- a/src/user/user_objects.cc +++ b/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 { diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 2f5970e8..40defc2f 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -15,6 +15,7 @@ #ifndef MUJOCO_SRC_USER_USER_OBJECTS_H_ #define MUJOCO_SRC_USER_USER_OBJECTS_H_ +#include #include #include #include @@ -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 vert_; // vertex data + mjResource* resource_ = nullptr; // resource for mesh file + std::vector vert_; // vertex data std::vector normal_; // normal data std::vector texcoord_; // texcoord data std::vector 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& Vert() const { return vert_; } - float Vert(int i) const { return vert_[i]; } + const std::vector& Vert() const { return vert_; } + double Vert(int i) const { return vert_[i]; } const std::vector& UserVert() const { return spec_vert_; } const std::vector& UserNormal() const { return spec_normal_; } const std::vector& 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 - void LoadSTL(mjResource* resource); // load mesh in STL BIN format - void LoadMSH(mjResource* resource); // load mesh in MSH BIN format + // load from OBJ, STL, or MSH file; throws mjCError on failure + void LoadFromResource(mjResource* resource, bool remove_repeated = false); - 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, - std::string_view asset_type); + void CacheMesh(mjCCache *cache, const mjResource* resource); + + // convert vertices to double precision and remove repeated vertices if requested + void ProcessVertices(const std::vector& 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 diff --git a/src/user/user_util.cc b/src/user/user_util.cc index 1c602ca8..843f0e40 100644 --- a/src/user/user_util.cc +++ b/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 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) { diff --git a/src/user/user_util.h b/src/user/user_util.h index 1d516ab8..653528a4 100644 --- a/src/user/user_util.h +++ b/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) -double mjuu_makenormal(double* normal, const float* a, const float* b, const float* c); +// compute normal vector to given triangle +template 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); diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 333e4da0..8c66b1aa 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/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); diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 284c6edd..2e1eb1e0 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -85,6 +85,7 @@ TEST_F(MjCMeshTest, UnknownMeshFormat) { )"; + std::vector 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 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(); - mj_defaultVFS(vfs.get()); + 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.get()); + mjModel* model = LoadModelFromString(xml, error, error_sz, &vfs); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("unsupported content type: 'model/unknown'")); - mj_deleteVFS(vfs.get()); + EXPECT_THAT(error, HasSubstr("unsupported mesh type: 'model/unknown'")); + mj_deleteVFS(&vfs); +} + +TEST_F(MjCMeshTest, LoadMSHWithInvalidContentType) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + + 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_THAT(model->body_inertia[4], model->body_inertia[10]); - EXPECT_THAT(model->body_inertia[5], model->body_inertia[11]); + EXPECT_NE(model->body_inertia[3], model->body_inertia[9]); + EXPECT_NE(model->body_inertia[4], model->body_inertia[10]); + 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]);