From efca1a76aafa75c67495106f6b8a35a53ad26e10 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Wed, 2 Aug 2023 14:42:49 +0100 Subject: [PATCH] Encapsulate mjCMesh data, apply Google C++ naming convention to class members (See https://google.github.io/styleguide/cppguide.html#Variable_Names), and fix a couple of typos. PiperOrigin-RevId: 553129088 Change-Id: Ic515154220af472f4fc90ca9ade92d86b205cc94 --- src/user/user_mesh.cc | 919 ++++++++++++++++++++--------------- src/user/user_model.cc | 70 +-- src/user/user_objects.cc | 11 +- src/user/user_objects.h | 165 ++++--- src/xml/xml_native_reader.cc | 22 +- src/xml/xml_native_writer.cc | 28 +- src/xml/xml_urdf.cc | 17 +- src/xml/xml_util.cc | 10 +- src/xml/xml_util.h | 2 +- 9 files changed, 712 insertions(+), 532 deletions(-) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 17b6d612..ff36557d 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #ifdef MUJOCO_TINYOBJLOADER_IMPL @@ -79,91 +80,8 @@ static mjtNum _triangle(mjtNum* normal, mjtNum* center, return len/2; } -//------------------ class mjCMesh implementation -------------------------------------------------- - -// constructor -mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) { - // set defaults - mjuu_setvec(refpos, 0, 0, 0); - mjuu_setvec(refquat, 1, 0, 0, 0); - mjuu_setvec(scale, 1, 1, 1); - smoothnormal = false; - file.clear(); - uservert.clear(); - usernormal.clear(); - usertexcoord.clear(); - userface.clear(); - userfacenormal.clear(); - userfacetexcoord.clear(); - useredge.clear(); - - // clear internal variables - mjuu_setvec(pos_surface, 0, 0, 0); - mjuu_setvec(pos_volume, 0, 0, 0); - mjuu_setvec(quat_surface, 1, 0, 0, 0); - mjuu_setvec(quat_volume, 1, 0, 0, 0); - mjuu_setvec(boxsz_surface, 0, 0, 0); - mjuu_setvec(boxsz_volume, 0, 0, 0); - mjuu_setvec(aabb, 1e10, 1e10, 1e10); - mjuu_setvec(aabb+3, -1e10, -1e10, -1e10); - nvert = 0; - nnormal = 0; - ntexcoord = 0; - nface = 0; - szgraph = 0; - vert = NULL; - normal = NULL; - center = NULL; - texcoord = NULL; - face = NULL; - facenormal = NULL; - facetexcoord = NULL; - graph = NULL; - needhull = false; - invalidorientation.first = -1; - invalidorientation.second = -1; - validarea = true; - validvolume = 1; - valideigenvalue = true; - validinequality = true; - processed = false; - - // reset to default if given - if (_def) { - *this = _def->mesh; - } - - // set model, def - model = _model; - def = (_def ? _def : (_model ? _model->defaults[0] : 0)); -} - - - -// destructor -mjCMesh::~mjCMesh() { - file.clear(); - uservert.clear(); - usernormal.clear(); - usertexcoord.clear(); - userface.clear(); - userfacenormal.clear(); - userfacetexcoord.clear(); - useredge.clear(); - - if (vert) mju_free(vert); - if (normal) mju_free(normal); - if (texcoord) mju_free(texcoord); - if (center) mju_free(center); - if (face) mju_free(face); - if (facenormal) mju_free(facenormal); - if (facetexcoord) mju_free(facetexcoord); - if (graph) mju_free(graph); -} - - - -template static T* VecToArray(std::vector& vector, bool clear = true){ +template +static T* VecToArray(std::vector& vector, bool clear = true){ if (vector.empty()) return nullptr; else { @@ -177,20 +95,179 @@ template static T* VecToArray(std::vector& vector, bool clear = } } +//------------------ class mjCMesh implementation -------------------------------------------------- + +mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) { + // set defaults + mjuu_setvec(refpos_, 0, 0, 0); + mjuu_setvec(refquat_, 1, 0, 0, 0); + mjuu_setvec(scale_, 1, 1, 1); + smoothnormal_ = false; + + // clear internal variables + mjuu_setvec(pos_surface_, 0, 0, 0); + mjuu_setvec(pos_volume_, 0, 0, 0); + mjuu_setvec(quat_surface_, 1, 0, 0, 0); + mjuu_setvec(quat_volume_, 1, 0, 0, 0); + mjuu_setvec(boxsz_surface_, 0, 0, 0); + mjuu_setvec(boxsz_volume_, 0, 0, 0); + mjuu_setvec(aabb_, 1e10, 1e10, 1e10); + mjuu_setvec(aabb_+3, -1e10, -1e10, -1e10); + nvert_ = 0; + nnormal_ = 0; + ntexcoord_ = 0; + nface_ = 0; + szgraph_ = 0; + vert_ = NULL; + normal_ = NULL; + center_ = NULL; + texcoord_ = NULL; + face_ = NULL; + facenormal_ = NULL; + facetexcoord_ = NULL; + graph_ = NULL; + needhull_ = false; + invalidorientation_.first = -1; + invalidorientation_.second = -1; + validarea_ = true; + validvolume_ = 1; + valideigenvalue_ = true; + validinequality_ = true; + processed_ = false; + + // reset to default if given + if (_def) { + *this = _def->mesh; + } + + // set model, def + model = _model; + def = (_def ? _def : (_model ? _model->defaults[0] : 0)); +} + + + +mjCMesh::~mjCMesh() { + if (vert_) mju_free(vert_); + if (normal_) mju_free(normal_); + if (texcoord_) mju_free(texcoord_); + if (center_) mju_free(center_); + if (face_) mju_free(face_); + if (facenormal_) mju_free(facenormal_); + if (facetexcoord_) mju_free(facetexcoord_); + if (graph_) mju_free(graph_); +} + + + +void mjCMesh::set_content_type(std::optional&& content_type) { + if (content_type.has_value()) { + content_type_ = std::move(content_type.value()); + } +} + + + +void mjCMesh::set_file(std::optional&& file) { + if (file.has_value()) { + file_ = std::move(file.value()); + } +} + + + +void mjCMesh::set_refpos(std::optional> refpos) { + if (refpos.has_value()) { + std::copy(refpos.value().begin(), refpos.value().end(), refpos_); + } +} + + + +void mjCMesh::set_refquat(std::optional> refquat) { + if (refquat.has_value()) { + std::copy(refquat.value().begin(), refquat.value().end(), refquat_); + } +} + + + +void mjCMesh::set_scale(std::optional> scale) { + if (scale.has_value()) { + set_scale(scale.value()); + } +} + + + +void mjCMesh::set_uservert(std::optional>&& uservert) { + if (uservert.has_value()) { + uservert_ = std::move(uservert.value()); + } +} + + + +void mjCMesh::set_usernormal(std::optional>&& usernormal) { + if (usernormal.has_value()) { + usernormal_ = std::move(usernormal.value()); + } +} + + + +void mjCMesh::set_usertexcoord(std::optional>&& usertexcoord) { + if (usertexcoord.has_value()) { + usertexcoord_ = std::move(usertexcoord.value()); + } +} + + + +void mjCMesh::set_userface(std::optional>&& userface) { + if (userface.has_value()) { + userface_ = std::move(userface.value()); + } +} + + + +void mjCMesh::set_file(const std::string& file) { + file_ = file; +} + + + +void mjCMesh::set_scale(std::array scale) { + std::copy(scale.begin(), scale.end(), scale_); +} + + + +void mjCMesh::set_smoothnormal(bool smoothnormal) { + smoothnormal_ = smoothnormal; +} + + + +void mjCMesh::set_needhull(bool needhull) { + needhull_ = needhull; +} + // compiler void mjCMesh::Compile(int vfs_provider) { // load file - if (!file.empty()) { + if (!file_.empty()) { // remove path from file if necessary if (model->strippath) { - file = mjuu_strippath(file); + file_ = mjuu_strippath(file_); } - std::string asset_type = GetAssetContentType(file, content_type); + std::string asset_type = GetAssetContentType(file_, content_type_); if (asset_type.empty()) { - throw mjCError(this, "unknown mesh content type for file: '%s'", file.c_str()); + throw mjCError(this, "unknown mesh content type for file: '%s'", file_.c_str()); } if (asset_type != "model/stl" && asset_type != "model/obj" @@ -198,7 +275,7 @@ void mjCMesh::Compile(int vfs_provider) { throw mjCError(this, "unsupported content type: '%s'", asset_type.c_str()); } - string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file); + string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file_); mjResource* resource = LoadResource(filename, vfs_provider); try { @@ -217,89 +294,89 @@ void mjCMesh::Compile(int vfs_provider) { } // copy user vertex - if (!uservert.empty()) { + if (!uservert_.empty()) { // check repeated - if (vert) { + if (vert_) { throw mjCError(this, "repeated vertex specification"); } // check size - if (uservert.size()<12) { + if (uservert_.size()<12) { throw mjCError(this, "at least 4 vertices required"); } - if (uservert.size()%3) { + if (uservert_.size()%3) { throw mjCError(this, "vertex data must be a multiple of 3"); } // copy from user - nvert = (int)uservert.size()/3; - vert = VecToArray(uservert, !file.empty()); + nvert_ = (int)uservert_.size()/3; + vert_ = VecToArray(uservert_, !file_.empty()); } // copy user normal - if (!usernormal.empty()) { + if (!usernormal_.empty()) { // check repeated - if (normal) { + if (normal_) { throw mjCError(this, "repeated normal specification"); } // check size - if (usernormal.size()%3) { + if (usernormal_.size()%3) { throw mjCError(this, "normal data must be a multiple of 3"); } // copy from user - nnormal = (int)usernormal.size()/3; - normal = VecToArray(usernormal, !file.empty()); + nnormal_ = (int)usernormal_.size()/3; + normal_ = VecToArray(usernormal_, !file_.empty()); } // copy user texcoord - if (!usertexcoord.empty()) { + if (!usertexcoord_.empty()) { // check repeated - if (texcoord) { + if (texcoord_) { throw mjCError(this, "repeated texcoord specification"); } // check size - if (usertexcoord.size()%2) { + if (usertexcoord_.size()%2) { throw mjCError(this, "texcoord must be a multiple of 2"); } // copy from user - ntexcoord = (int)usertexcoord.size()/2; - texcoord = VecToArray(usertexcoord, !file.empty()); + ntexcoord_ = (int)usertexcoord_.size()/2; + texcoord_ = VecToArray(usertexcoord_, !file_.empty()); } // copy user face - if (!userface.empty()) { + if (!userface_.empty()) { // check repeated - if (face) { + if (face_) { throw mjCError(this, "repeated face specification"); } // check size - if (userface.size()%3) { + if (userface_.size()%3) { throw mjCError(this, "face data must be a multiple of 3"); } // check vertices exist - for (int i=0; i= nvert || userface[i] < 0) { + for (int i=0; i= nvert_ || userface_[i] < 0) { throw mjCError(this, "index in face does not exist in vertex array"); } } // create half-edge structure (if mesh was in XML) - if (useredge.empty()) { - for (int i=0; isqrt(mjMINVAL)) { - useredge.push_back(std::pair(v0, v1)); - useredge.push_back(std::pair(v1, v2)); - useredge.push_back(std::pair(v2, v0)); + if (_triangle(normal, nullptr, vert_+3*v0, vert_+3*v1, vert_+3*v2)>sqrt(mjMINVAL)) { + useredge_.push_back(std::pair(v0, v1)); + useredge_.push_back(std::pair(v1, v2)); + useredge_.push_back(std::pair(v2, v0)); } else { // TODO(b/255525326) } @@ -307,93 +384,93 @@ void mjCMesh::Compile(int vfs_provider) { } // copy from user - nface = (int)userface.size()/3; - face = VecToArray(userface, !file.empty()); + nface_ = (int)userface_.size()/3; + face_ = VecToArray(userface_, !file_.empty()); // check vertices exist - for (auto vertex_index : userface) { - if (vertex_index>=nvert || vertex_index < 0) { + for (auto vertex_index : userface_) { + if (vertex_index>=nvert_ || vertex_index < 0) { throw mjCError(this, "found index in userface that exceeds uservert size."); } } } // check for inconsistent face orientations - if (!useredge.empty()) { - std::stable_sort(useredge.begin(), useredge.end()); - auto iterator = std::adjacent_find(useredge.begin(), useredge.end()); - if (iterator != useredge.end()) { - invalidorientation.first = iterator->first+1; - invalidorientation.second = iterator->second+1; + if (!useredge_.empty()) { + std::stable_sort(useredge_.begin(), useredge_.end()); + auto iterator = std::adjacent_find(useredge_.begin(), useredge_.end()); + if (iterator != useredge_.end()) { + invalidorientation_.first = iterator->first+1; + invalidorientation_.second = iterator->second+1; } } // require vertices - if (!vert) { + if (!vert_) { throw mjCError(this, "no vertices"); } // make graph describing convex hull - if ((model->convexhull && needhull) || !face) { + if ((model->convexhull && needhull_) || !face_) { MakeGraph(); } // no faces: copy from convex hull - if (!face) { + if (!face_) { CopyGraph(); } // no normals: make - if (!normal) { + if (!normal_) { MakeNormal(); } // copy user normal indices - if (!userfacenormal.empty()) { + if (!userfacenormal_.empty()) { // check repeated - if (facenormal) { + if (facenormal_) { throw mjCError(this, "repeated facenormal specification"); } - if (userfacenormal.size()!=3*nface) { + if (userfacenormal_.size()!=3*nface_) { throw mjCError(this, "face data must have the same size as face normal data"); } - facenormal = VecToArray(userfacenormal, !file.empty()); + facenormal_ = VecToArray(userfacenormal_, !file_.empty()); } // copy user texcoord - if (!userfacetexcoord.empty()) { + if (!userfacetexcoord_.empty()) { // check repeated - if (facetexcoord) { + if (facetexcoord_) { throw mjCError(this, "repeated facetexcoord specification"); } - facetexcoord = VecToArray(userfacetexcoord, !file.empty()); + facetexcoord_ = VecToArray(userfacetexcoord_, !file_.empty()); } // facenormal might not exist if usernormal was specified - if (!facenormal) { - facenormal = (int*) mju_malloc(3*nface*sizeof(int)); - memcpy(facenormal, face, 3*nface*sizeof(int)); + if (!facenormal_) { + facenormal_ = (int*) mju_malloc(3*nface_*sizeof(int)); + memcpy(facenormal_, face_, 3*nface_*sizeof(int)); } // scale, center, orient, compute mass and inertia Process(); - processed = true; + processed_ = true; // no radii: make - if (!center) { + if (!center_) { MakeCenter(); } // make bounding volume hierarchy - if (tree.bvh.empty()) { - face_aabb.assign(6*nface, 0); - for (int i=0; isize[0] = 0; - for (int i=0; isize[0] = mjMAX(geom->size[0], dst); } @@ -514,8 +639,8 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) { // find maximum distance in XY, separately in Z geom->size[0] = 0; geom->size[1] = 0; - for (int i=0; isize[0] = mjMAX(geom->size[0], dst); @@ -528,9 +653,9 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) { // special handling of capsule: consider curved cap if (geom->type==mjGEOM_CAPSULE) { geom->size[1] = 0; - for (int i=0; isize[0] = aabb[3] - cen[0]; - geom->size[1] = aabb[4] - cen[1]; - geom->size[2] = aabb[5] - cen[2]; + geom->size[0] = aabb_[3] - cen[0]; + geom->size[1] = aabb_[4] - cen[1]; + geom->size[2] = aabb_[5] - cen[2]; break; default: @@ -582,20 +707,20 @@ void mjCMesh::RemoveRepeated() { int repeated = 0; // allocate sort and redirection indices, set to identity - auto index = std::unique_ptr(new int[nvert]); - auto redirect = std::unique_ptr(new int[nvert]); - for (int i=0; i < nvert; i++) { + auto index = std::unique_ptr(new int[nvert_]); + auto redirect = std::unique_ptr(new int[nvert_]); + for (int i=0; i < nvert_; i++) { index[i] = redirect[i] = i; } // sort vertices - mjQUICKSORT(index.get(), nvert, sizeof(int), vertcompare, vert); + mjQUICKSORT(index.get(), nvert_, sizeof(int), vertcompare, 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]) { + 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++; } @@ -604,7 +729,7 @@ void mjCMesh::RemoveRepeated() { // compress vertices, change face data if (repeated) { // track redirections until non-redirected vertex, set - for (int i=0; i=nvert-repeated) { + if (face_[i]<0 || face_[i]>=nvert_-repeated) { throw mjCError( this, "error removing vertices from mesh '%s'", name.c_str()); } @@ -637,13 +762,13 @@ void mjCMesh::RemoveRepeated() { } // correct vertex count - nvert -= repeated; + nvert_ -= repeated; // resize vert if any vertices were removed if (repeated) { - float* old = vert; - vert = (float*) mju_malloc(3*nvert*sizeof(float)); - memcpy(vert, old, 3*nvert*sizeof(float)); + float* old = vert_; + vert_ = (float*) mju_malloc(3*nvert_*sizeof(float)); + memcpy(vert_, old, 3*nvert_*sizeof(float)); mju_free(old); } } @@ -667,13 +792,13 @@ void mjCMesh::LoadOBJ(mjResource* resource) { } const auto& attrib = objReader.GetAttrib(); - uservert = attrib.vertices; // copy from one std::vector to another - usernormal = attrib.normals; - usertexcoord = attrib.texcoords; + uservert_ = attrib.vertices; // copy from one std::vector to another + usernormal_ = attrib.normals; + usertexcoord_ = attrib.texcoords; 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; @@ -700,28 +825,28 @@ void mjCMesh::LoadOBJ(mjResource* resource) { // for each vertex, store index, normal, and texcoord for (const auto& mesh_index : face_indices) { - userface.push_back(mesh_index.vertex_index); + userface_.push_back(mesh_index.vertex_index); - if (!usernormal.empty()) { - userfacenormal.push_back(mesh_index.normal_index); + if (!usernormal_.empty()) { + userfacenormal_.push_back(mesh_index.normal_index); } - if (!usertexcoord.empty()) { - userfacetexcoord.push_back(mesh_index.texcoord_index); + if (!usertexcoord_.empty()) { + userfacetexcoord_.push_back(mesh_index.texcoord_index); } } } // flip the second texcoord - for (int i=0; i0); + bool righthand = (scale_[0]*scale_[1]*scale_[2]>0); // get file data in buffer char* buffer = 0; @@ -740,15 +865,15 @@ void mjCMesh::LoadSTL(mjResource* resource) { } // get number of triangles, check bounds - nface = *(unsigned int*)(buffer+80); - if (nface<1 || nface>200000) { + nface_ = *(unsigned int*)(buffer+80); + if (nface_<1 || nface_>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); } // check remaining buffer size - if (nface*50 != buffer_sz-84) { + if (nface_*50 != buffer_sz-84) { throw mjCError(this, "STL file '%s' has wrong size; perhaps this is an ASCII file?", resource->name); @@ -758,11 +883,11 @@ void mjCMesh::LoadSTL(mjResource* resource) { const char* stl = buffer + 84; // allocate face and vertex data - face = (int*) mju_malloc(3*nface*sizeof(int)); - vert = (float*) mju_malloc(9*nface*sizeof(float)); + face_ = (int*) mju_malloc(3*nface_*sizeof(int)); + vert_ = (float*) mju_malloc(9*nface_*sizeof(float)); // add vertices and faces, including repeated for now - for (int i=0; i0); + bool righthand = (scale_[0]*scale_[1]*scale_[2]>0); // get file data in buffer char* buffer = 0; @@ -818,58 +943,58 @@ void mjCMesh::LoadMSH(mjResource* resource) { } // get sizes from header - nvert = ((int*)buffer)[0]; - nnormal = ((int*)buffer)[1]; - ntexcoord = ((int*)buffer)[2]; - nface = ((int*)buffer)[3]; + nvert_ = ((int*)buffer)[0]; + nnormal_ = ((int*)buffer)[1]; + ntexcoord_ = ((int*)buffer)[2]; + nface_ = ((int*)buffer)[3]; // check sizes - if (nvert<4 || nface<0 || nnormal<0 || ntexcoord<0 || - (nnormal>0 && nnormal!=nvert) || - (ntexcoord>0 && ntexcoord!=nvert)) { + if (nvert_<4 || nface_<0 || nnormal_<0 || ntexcoord_<0 || + (nnormal_>0 && nnormal_!=nvert_) || + (ntexcoord_>0 && ntexcoord_!=nvert_)) { throw mjCError(this, "invalid sizes in MSH file '%s'", resource->name); } // check file size - if (buffer_sz != 4*sizeof(int) + 3*nvert*sizeof(float) + 3*nnormal*sizeof(float) + - 2*ntexcoord*sizeof(float) + 3*nface*sizeof(int)) { + if (buffer_sz != 4*sizeof(int) + 3*nvert_*sizeof(float) + 3*nnormal_*sizeof(float) + + 2*ntexcoord_*sizeof(float) + 3*nface_*sizeof(int)) { throw mjCError(this, "unexpected file size in MSH file '%s'", resource->name); } // allocate and copy float* fdata = (float*)(((int*)buffer) + 4); - if (nvert) { - vert = (float*) mju_malloc(3*nvert*sizeof(float)); - memcpy(vert, fdata, 3*nvert*sizeof(float)); - fdata += 3*nvert; + if (nvert_) { + vert_ = (float*) mju_malloc(3*nvert_*sizeof(float)); + memcpy(vert_, fdata, 3*nvert_*sizeof(float)); + fdata += 3*nvert_; } - if (nnormal) { - normal = (float*) mju_malloc(3*nvert*sizeof(float)); - memcpy(normal, fdata, 3*nvert*sizeof(float)); - fdata += 3*nvert; + if (nnormal_) { + normal_ = (float*) mju_malloc(3*nvert_*sizeof(float)); + memcpy(normal_, fdata, 3*nvert_*sizeof(float)); + fdata += 3*nvert_; } - if (ntexcoord) { - texcoord = (float*) mju_malloc(2*nvert*sizeof(float)); - memcpy(texcoord, fdata, 2*nvert*sizeof(float)); - fdata += 2*nvert; + if (ntexcoord_) { + texcoord_ = (float*) mju_malloc(2*nvert_*sizeof(float)); + memcpy(texcoord_, fdata, 2*nvert_*sizeof(float)); + fdata += 2*nvert_; } - if (nface) { - face = (int*) mju_malloc(3*nface*sizeof(int)); - facenormal = (int*) mju_malloc(3*nface*sizeof(int)); - memcpy(face, fdata, 3*nface*sizeof(int)); - memcpy(facenormal, fdata, 3*nface*sizeof(int)); + if (nface_) { + face_ = (int*) mju_malloc(3*nface_*sizeof(int)); + facenormal_ = (int*) mju_malloc(3*nface_*sizeof(int)); + memcpy(face_, fdata, 3*nface_*sizeof(int)); + memcpy(facenormal_, fdata, 3*nface_*sizeof(int)); } - if (nface && texcoord) { - facetexcoord = (int*) mju_malloc(3*nface*sizeof(int)); - memcpy(facetexcoord, fdata, 3*nface*sizeof(int)); + if (nface_ && texcoord_) { + facetexcoord_= (int*) mju_malloc(3*nface_*sizeof(int)); + memcpy(facetexcoord_, fdata, 3*nface_*sizeof(int)); } // rearange face data if left-handed scaling - if (nface && !righthand) { - for (int i=0; imjMINVAL) { float scl = 1/sqrtf(len); - normal[3*i] *= scl; - normal[3*i+1] *= scl; - normal[3*i+2] *= scl; + normal_[3*i] *= 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; + normal_[3*i+1] = 0; + normal_[3*i+2] = 1; } } } @@ -986,16 +1111,16 @@ void mjCMesh::ComputeFaceCentroid(double facecen[3]) { double nrm[3]; double cen[3]; - for (int i=0; i=nvert) { + if (face_[3*i+j]<0 || face_[3*i+j]>=nvert_) { throw mjCError(this, "vertex index out of range in %s (index = %d)", name.c_str(), i); } } // get area and center - double a = _triangle(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]); + double a = _triangle(nrm, cen, vert_+3*face_[3*i], vert_+3*face_[3*i+1], vert_+3*face_[3*i+2]); // accumulate for (int j=0; j<3; j++) { @@ -1006,7 +1131,7 @@ void mjCMesh::ComputeFaceCentroid(double facecen[3]) { // require positive area if (area < mjMINVAL) { - validarea = false; + validarea_ = false; return; } @@ -1046,7 +1171,7 @@ void mjCMesh::Process() { // if volume is still invalid, skip the rest of the computations if (GetVolumeRef(type) < mjMINVAL) { - validvolume = GetVolumeRef(type) < 0 ? -1 : 0; + validvolume_ = GetVolumeRef(type) < 0 ? -1 : 0; continue; } @@ -1057,10 +1182,10 @@ void mjCMesh::Process() { mjuu_copyvec(GetPosPtr(type), CoM, 3); // re-center mesh at CoM - if (type==mjVOLUME_MESH || validvolume<=0) { - for (int i=0; i0) { + if (type==mjSHELL_MESH && validvolume_>0) { mju_copy4(GetQuatPtr(type), GetQuatPtr(mjVOLUME_MESH)); continue; } @@ -1145,26 +1270,26 @@ void mjCMesh::Process() { double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]}; double mat[9]; mjuu_quat2mat(mat, neg); - for (int i=0; i=0 || invalidorientation.second>=0) + if (invalidorientation_.first>=0 || invalidorientation_.second>=0) throw mjCError(this, "faces of mesh '%s' have inconsistent orientation. Please check the " "faces containing the vertices %d and %d.", - name.c_str(), invalidorientation.first, invalidorientation.second); - if (!validarea && type==mjSHELL_MESH) + name.c_str(), invalidorientation_.first, invalidorientation_.second); + if (!validarea_ && type==mjSHELL_MESH) throw mjCError(this, "mesh surface area is too small: %s", name.c_str()); - if (validvolume<0 && type==mjVOLUME_MESH) + if (validvolume_<0 && type==mjVOLUME_MESH) throw mjCError(this, "mesh volume is negative (misoriented triangles): %s", name.c_str()); - if (!validvolume && type==mjVOLUME_MESH) + if (!validvolume_ && type==mjVOLUME_MESH) throw mjCError(this, "mesh volume is too small: %s", name.c_str()); - if (!valideigenvalue) + if (!valideigenvalue_) throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str()); - if (!validinequality) + if (!validinequality_) throw mjCError(this, "eigenvalues of mesh inertia violate A + B >= C: %s", name.c_str()); } @@ -1197,13 +1322,13 @@ void mjCMesh::CheckMesh(mjtMeshType type) { // get inertia pointer double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) { CheckMesh(type); - return type==mjSHELL_MESH ? boxsz_surface : boxsz_volume; + return type==mjSHELL_MESH ? boxsz_surface_ : boxsz_volume_; } double& mjCMesh::GetVolumeRef(mjtMeshType type) { CheckMesh(type); - return type==mjSHELL_MESH ? surface : volume; + return type==mjSHELL_MESH ? surface_ : volume_; } @@ -1216,17 +1341,17 @@ void mjCMesh::MakeGraph(void) { char qhopt[10] = "qhull Qt"; // graph not needed for small meshes - if (nvert<4) { + if (nvert_ < 4) { return; } // convert mesh data to double - data = (double*) mju_malloc(3*nvert*sizeof(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++) { - data[i] = (double)vert[i]; + for (int i=0; i<3*nvert_; i++) { + data[i] = (double)vert_[i]; } qhT qh_qh; @@ -1242,7 +1367,7 @@ void mjCMesh::MakeGraph(void) { if (!exitcode) { // actual init qh_initflags(qh, qhopt); - qh_init_B(qh, data, nvert, 3, False); + qh_init_B(qh, data, nvert_, 3, False); // construct convex hull qh_qhull(qh); @@ -1254,16 +1379,16 @@ void mjCMesh::MakeGraph(void) { // edge_localid[numvert+3*numface], face_globalid[3*numface] int numvert = qh->num_vertices; int numface = qh->num_facets; - szgraph = 2 + 3*numvert + 6*numface; - graph = (int*) mju_malloc(szgraph*sizeof(int)); - graph[0] = numvert; - graph[1] = numface; + szgraph_ = 2 + 3*numvert + 6*numface; + graph_ = (int*) mju_malloc(szgraph_*sizeof(int)); + graph_[0] = numvert; + graph_[1] = numface; - // pointers for conveniece - int* vert_edgeadr = graph + 2; - int* vert_globalid = graph + 2 + numvert; - int* edge_localid = graph + 2 + 2*numvert; - int* face_globalid = graph + 2 + 3*numvert + 3*numface; + // pointers for convenience + int* vert_edgeadr = graph_ + 2; + int* vert_globalid = graph_ + 2 + numvert; + int* edge_localid = graph_ + 2 + 2*numvert; + int* face_globalid = graph_ + 2 + 3*numvert + 3*numface; // fill in graph data int i = adr = 0; @@ -1271,7 +1396,7 @@ void mjCMesh::MakeGraph(void) { FORALLvertices { // point id of this vertex, check int pid = qh_pointid(qh, vertex->point); - if (pid<0 || pid>=nvert) { + if (pid<0 || pid>=nvert_) { ok = 0; break; } @@ -1280,7 +1405,7 @@ void mjCMesh::MakeGraph(void) { vert_edgeadr[i] = adr; vert_globalid[i] = pid; - // process neighoring faces and their vertices + // process neighboring faces and their vertices int start = adr; FOREACHsetelement_(facetT, vertex->neighbors, facet) { int cnt = 0; @@ -1289,7 +1414,7 @@ void mjCMesh::MakeGraph(void) { // point id of face vertex, check int pid1 = qh_pointid(qh, vertex1->point); - if (pid1<0 || pid1>=nvert) { + if (pid1<0 || pid1>=nvert_) { ok = 0; break; } @@ -1357,9 +1482,9 @@ void mjCMesh::MakeGraph(void) { // bad graph: delete if (!ok) { - szgraph = 0; - mju_free(graph); - graph = 0; + szgraph_ = 0; + mju_free(graph_); + graph_ = 0; mju_warning("Could not construct convex hull graph"); } @@ -1389,9 +1514,9 @@ void mjCMesh::MakeGraph(void) { qh_freeqhull(qh, !qh_ALL); qh_memfreeshort(qh, &curlong, &totlong); mju_free(data); - if (graph) { - mju_free(graph); - szgraph = 0; + if (graph_) { + mju_free(graph_); + szgraph_ = 0; } throw mjCError(this, "qhull error"); @@ -1403,24 +1528,24 @@ void mjCMesh::MakeGraph(void) { // copy graph into face data void mjCMesh::CopyGraph(void) { // only if face data is missing - if (face) { + if (face_) { return; } // get info from graph, allocate - int numvert = graph[0]; - nface = graph[1]; - face = (int*) mju_malloc(3*nface*sizeof(int)); + int numvert = graph_[0]; + nface_ = graph_[1]; + face_ = (int*) mju_malloc(3*nface_*sizeof(int)); // copy faces - for (int i=0; imjMINVAL) for (int j=0; j<3; j++) { - normal[3*i+j] /= len; + normal_[3*i+j] /= len; } else { - normal[3*i] = normal[3*i+1] = 0; - normal[3*i+2] = 1; + normal_[3*i] = normal_[3*i+1] = 0; + normal_[3*i+2] = 1; } } } @@ -1542,23 +1667,23 @@ void mjCMesh::MakeNormal(void) { // compute face circumradii void mjCMesh::MakeCenter(void) { - if (center) { + if (center_) { return; } // allocate and clear - center = (double*) mju_malloc(3*nface*sizeof(double)); - memset(center, 0, 3*nface*sizeof(double)); + center_ = (double*) mju_malloc(3*nface_*sizeof(double)); + memset(center_, 0, 3*nface_*sizeof(double)); - for (int i=0; iname.empty()) { - stripped = mjuu_strippath(meshes[i]->file); + stripped = mjuu_strippath(meshes[i]->file()); meshes[i]->name = mjuu_stripext(stripped); // name cannot be empty @@ -934,12 +934,12 @@ void mjCModel::SetSizes(void) { // nmeshvert, nmeshface, nmeshtexcoord, nmeshgraph for (int i=0; invert; - nmeshnormal += meshes[i]->nnormal; - nmeshface += meshes[i]->nface; - nmeshtexcoord += (meshes[i]->texcoord ? meshes[i]->ntexcoord : 0); - nmeshgraph += meshes[i]->szgraph; - nbvh += meshes[i]->tree.nbvh; + nmeshvert += meshes[i]->nvert(); + nmeshnormal += meshes[i]->nnormal(); + nmeshface += meshes[i]->nface(); + nmeshtexcoord += (meshes[i]->HasTexcoord() ? meshes[i]->ntexcoord() : 0); + nmeshgraph += meshes[i]->szgraph(); + nbvh += meshes[i]->tree().nbvh; } // nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert @@ -1712,43 +1712,43 @@ void mjCModel::CopyObjects(mjModel* m) { // set fields m->mesh_vertadr[i] = vert_adr; - m->mesh_vertnum[i] = pme->nvert; + m->mesh_vertnum[i] = pme->nvert(); m->mesh_normaladr[i] = normal_adr; - m->mesh_normalnum[i] = pme->nnormal; - m->mesh_texcoordadr[i] = (pme->texcoord ? texcoord_adr : -1); - m->mesh_texcoordnum[i] = pme->ntexcoord; + m->mesh_normalnum[i] = pme->nnormal(); + m->mesh_texcoordadr[i] = (pme->HasTexcoord() ? texcoord_adr : -1); + m->mesh_texcoordnum[i] = pme->ntexcoord(); m->mesh_faceadr[i] = face_adr; - m->mesh_facenum[i] = pme->nface; - m->mesh_graphadr[i] = (pme->szgraph ? graph_adr : -1); + m->mesh_facenum[i] = pme->nface(); + m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1); m->mesh_bvhadr[i] = bvh_adr; - m->mesh_bvhnum[i] = pme->tree.nbvh; + m->mesh_bvhnum[i] = pme->tree().nbvh; // copy vertices, normals, faces, texcoords, aux data - memcpy(m->mesh_vert + 3*vert_adr, pme->vert, 3*pme->nvert*sizeof(float)); - memcpy(m->mesh_normal + 3*normal_adr, pme->normal, 3*pme->nnormal*sizeof(float)); - memcpy(m->mesh_face + 3*face_adr, pme->face, 3*pme->nface*sizeof(float)); - memcpy(m->mesh_facenormal + 3*face_adr, pme->facenormal, 3*pme->nface*sizeof(int)); - if (pme->texcoord) { - memcpy(m->mesh_texcoord + 2*texcoord_adr, pme->texcoord, 2*pme->ntexcoord*sizeof(float)); - memcpy(m->mesh_facetexcoord + 3*face_adr, pme->facetexcoord, 3*pme->nface*sizeof(int)); + pme->CopyVert(m->mesh_vert + 3*vert_adr); + pme->CopyNormal(m->mesh_normal + 3*normal_adr); + pme->CopyFace(m->mesh_face + 3*face_adr); + pme->CopyFaceNormal(m->mesh_facenormal + 3*face_adr); + if (pme->HasTexcoord()) { + pme->CopyTexcoord(m->mesh_texcoord + 2*texcoord_adr); + pme->CopyFaceTexcoord(m->mesh_facetexcoord + 3*face_adr); } else { - memset(m->mesh_facetexcoord + 3*face_adr, 0, 3*pme->nface*sizeof(int)); + memset(m->mesh_facetexcoord + 3*face_adr, 0, 3*pme->nface()*sizeof(int)); } - if (pme->szgraph) { - memcpy(m->mesh_graph + graph_adr, pme->graph, pme->szgraph*sizeof(int)); + if (pme->szgraph()) { + pme->CopyGraph(m->mesh_graph + graph_adr); } - memcpy(m->bvh_aabb + 6*bvh_adr, pme->tree.bvh.data(), 6*pme->tree.nbvh*sizeof(mjtNum)); - memcpy(m->bvh_child + 2*bvh_adr, pme->tree.child.data(), 2*pme->tree.nbvh*sizeof(int)); - memcpy(m->bvh_depth + bvh_adr, pme->tree.level.data(), pme->tree.nbvh*sizeof(int)); - memcpy(m->bvh_geomid + bvh_adr, pme->tree.nodeid.data(), pme->tree.nbvh*sizeof(int)); + memcpy(m->bvh_aabb + 6*bvh_adr, pme->tree().bvh.data(), 6*pme->tree().nbvh*sizeof(mjtNum)); + memcpy(m->bvh_child + 2*bvh_adr, pme->tree().child.data(), 2*pme->tree().nbvh*sizeof(int)); + memcpy(m->bvh_depth + bvh_adr, pme->tree().level.data(), pme->tree().nbvh*sizeof(int)); + memcpy(m->bvh_geomid + bvh_adr, pme->tree().nodeid.data(), pme->tree().nbvh*sizeof(int)); // advance counters - vert_adr += pme->nvert; - normal_adr += pme->nnormal; - texcoord_adr += (pme->texcoord ? pme->ntexcoord : 0); - face_adr += pme->nface; - graph_adr += pme->szgraph; - bvh_adr += pme->tree.nbvh; + vert_adr += pme->nvert(); + normal_adr += pme->nnormal(); + texcoord_adr += (pme->HasTexcoord() ? pme->ntexcoord() : 0); + face_adr += pme->nface(); + graph_adr += pme->szgraph(); + bvh_adr += pme->tree().nbvh; } // skins @@ -2535,7 +2535,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, int vfs_provider) { for (int i=0; imeshid>=0 && geoms[i]->type==mjGEOM_MESH && (geoms[i]->contype || geoms[i]->conaffinity)) { - meshes[geoms[i]->meshid]->needhull = true; + meshes[geoms[i]->meshid]->set_needhull(true); } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 27796aec..b55f6553 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -1302,7 +1302,8 @@ double mjCGeom::GetVolume(void) { if (model->exactmeshinertia || typeinertia==mjSHELL_MESH) { return pmesh->GetVolumeRef(typeinertia); } else { - return pmesh->boxsz_volume[0]*pmesh->boxsz_volume[1]*pmesh->boxsz_volume[2]*8; + const double* boxsz_volume = pmesh->boxsz_volume(); + return boxsz_volume[0]*boxsz_volume[1]*boxsz_volume[2]*8; } } @@ -1419,7 +1420,7 @@ void mjCGeom::SetInertia(void) { // compute radius of bounding sphere double mjCGeom::GetRBound(void) { - double* aabb; + const double* aabb; double haabb[3] = {0}; switch (type) { @@ -1439,7 +1440,7 @@ double mjCGeom::GetRBound(void) { return sqrt(size[0]*size[0]+size[1]*size[1]+size[2]*size[2]); case mjGEOM_MESH: - aabb = model->meshes[meshid]->aabb; + aabb = model->meshes[meshid]->aabb(); haabb[0] = mjMAX(fabs(aabb[0]), fabs(aabb[3])); haabb[1] = mjMAX(fabs(aabb[1]), fabs(aabb[4])); haabb[2] = mjMAX(fabs(aabb[2]), fabs(aabb[5])); @@ -1585,7 +1586,7 @@ void mjCGeom::ComputeAABB() { break; case mjGEOM_MESH: - mjuu_copyvec(aabb, model->meshes[meshid]->aabb, 6); + mjuu_copyvec(aabb, model->meshes[meshid]->aabb(), 6); break; case mjGEOM_PLANE: @@ -1752,7 +1753,7 @@ void mjCGeom::Compile(void) { size[1] = model->hfields[hfieldid]->size[1]; size[2] = 0.5*(model->hfields[hfieldid]->size[2]+model->hfields[hfieldid]->size[3]); } else if (type==mjGEOM_MESH) { - double* aabb = model->meshes[meshid]->aabb; + const double* aabb = model->meshes[meshid]->aabb(); size[0] = mjMAX(fabs(aabb[0]), fabs(aabb[3])); size[1] = mjMAX(fabs(aabb[1]), fabs(aabb[4])); size[2] = mjMAX(fabs(aabb[2]), fabs(aabb[5])); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index a3489995..0b378252 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -15,7 +15,9 @@ #ifndef MUJOCO_SRC_USER_USER_OBJECTS_H_ #define MUJOCO_SRC_USER_USER_OBJECTS_H_ +#include #include +#include #include #include @@ -518,42 +520,93 @@ class mjCLight : public mjCBase { // Describes a mesh class mjCMesh: public mjCBase { - friend class mjCDef; - friend class mjCGeom; - friend class mjCBody; - friend class mjCSkin; - friend class mjCModel; - friend class mjXWriter; - public: + mjCMesh(mjCModel* = 0, mjCDef* = 0); + ~mjCMesh(); + + // public getters + const std::string& content_type() const { return content_type_; } + const std::string& file() const { return file_; } + const double* refpos() const { return refpos_; } + const double* refquat() const { return refquat_; } + const double* scale() const { return scale_; } + bool smoothnormal() const { return smoothnormal_; } + + // public getters for user data + const std::vector& uservert() const { return uservert_; } + const std::vector& usernormal() const { return usernormal_; } + const std::vector& usertexcoord() const { return usertexcoord_; } + const std::vector& userface() const { return userface_; } + + // mesh properites computed by Compile + const double* boxsz_volume() const { return boxsz_volume_; } + const double* aabb() const { return aabb_; } + + // number of vertices, normals, texture coordinates, and faces + int nvert() const { return nvert_; } + int nnormal() const { return nnormal_; } + int ntexcoord() const { return ntexcoord_; } + int nface() const { return nface_; } + + // return size of graph data in ints + int szgraph() const { return szgraph_; } + + // bounding volume hierarchy tree + const mjCBoundingVolumeHierarchy& tree() { return tree_; } + + // general setters + void set_file(const std::string& file); + void set_scale(std::array scale); + void set_smoothnormal(bool smoothnormal); + void set_needhull(bool needhull); + + // setters used in reading XML attributes (no-op if empty optional) + void set_content_type(std::optional&& content_type); + void set_file(std::optional&& file); + void set_refpos(std::optional> refpos); + void set_refquat(std::optional> refquat); + void set_scale(std::optional> scale); + + void set_uservert(std::optional>&& uservert); + void set_usernormal(std::optional>&& usernormal); + void set_usertexcoord(std::optional>&& usertexcoord); + void set_userface(std::optional>&& userface); + + void Compile(int vfs_provider); // compiler double* GetPosPtr(mjtMeshType type); // get position double* GetQuatPtr(mjtMeshType type); // get orientation double* GetInertiaBoxPtr(mjtMeshType type); // get inertia box double& GetVolumeRef(mjtMeshType type); // get volume void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom + bool HasTexcoord() const; // texcoord not null + + void CopyVert(float* arr) const; // copy vert data into array + void CopyNormal(float* arr) const; // copy normal data into array + void CopyFace(int* arr) const; // copy face data into array + void CopyFaceNormal(int* arr) const; // copy face normal data into array + void CopyFaceTexcoord(int* arr) const; // copy face texcoord data into array + void CopyTexcoord(float* arr) const; // copy texcoord data into array + void CopyGraph(int* arr) const; // copy graph data into array // returns a bounding volume given a face mjCBoundingVolume GetBoundingVolume(int faceid); - std::string content_type; // content type of file - std::string file; // mesh file - double refpos[3]; // reference position (translate) - double refquat[4]; // reference orientation (rotate) - double scale[3]; // rescale mesh - bool smoothnormal; // do not exclude large-angle faces from normals - - std::vector uservert; // user vertex data - std::vector usernormal; // user normal data - std::vector usertexcoord; // user texcoord data - std::vector userface; // user vertex indices - std::vector userfacenormal; // user normal indices - std::vector userfacetexcoord; // user texcoord indices - std::vector< std::pair > useredge; // user half-edge data - private: - mjCMesh(mjCModel* = 0, mjCDef* = 0); // constructor - ~mjCMesh(); // destructor - void Compile(int vfs_provider); // compiler + std::string content_type_; // content type of file + std::string file_; // mesh file + double refpos_[3]; // reference position (translate) + double refquat_[4]; // reference orientation (rotate) + double scale_[3]; // rescale mesh + bool smoothnormal_; // do not exclude large-angle faces from normals + + std::vector uservert_; // user vertex data + std::vector usernormal_; // user normal data + std::vector usertexcoord_; // user texcoord data + std::vector userface_; // user vertex indices + std::vector userfacenormal_; // user normal indices + std::vector userfacetexcoord_; // user texcoord indices + std::vector< std::pair > useredge_; // user half-edge data + 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 @@ -572,43 +625,43 @@ class mjCMesh: public mjCBase { bool exactmeshinertia); // mesh properties that indicate a well-formed mesh - std::pair invalidorientation; // indices of invalid edge; -1 if none - bool validarea; // false if the area is too small - int validvolume; // 0: volume is too small, -1: volume is negative - bool valideigenvalue; // false if inertia eigenvalue is too small - bool validinequality; // false if inertia inequality is not satisfied - bool processed; // false if the mesh has not been processed yet + std::pair invalidorientation_; // indices of invalid edge; -1 if none + bool validarea_; // false if the area is too small + int validvolume_; // 0: volume is too small, -1: volume is negative + bool valideigenvalue_; // false if inertia eigenvalue is too small + bool validinequality_; // false if inertia inequality is not satisfied + bool processed_; // false if the mesh has not been processed yet // mesh properties computed by Compile - double pos_volume[3]; // CoM position - double pos_surface[3]; // CoM position - double quat_volume[4]; // inertia orientation - double quat_surface[4]; // inertia orientation - double boxsz_volume[3]; // half-sizes of equivalent inertia box (volume) - double boxsz_surface[3]; // half-sizes of equivalent inertia box (surface) - double aabb[6]; // axis-aligned bounding box - double volume; // volume of the mesh - double surface; // surface of the mesh + double pos_volume_[3]; // CoM position + double pos_surface_[3]; // CoM position + double quat_volume_[4]; // inertia orientation + double quat_surface_[4]; // inertia orientation + double boxsz_volume_[3]; // half-sizes of equivalent inertia box (volume) + double boxsz_surface_[3]; // half-sizes of equivalent inertia box (surface) + double aabb_[6]; // axis-aligned bounding box + double volume_; // volume of the mesh + double surface_; // surface of the mesh // mesh data to be copied into mjModel - int nvert; // number of vertices - int nnormal; // number of normals - int ntexcoord; // number of texcoords - int nface; // number of faces - int szgraph; // size of graph data in ints - float* vert; // vertex data (3*nvert), relative to (pos, quat) - float* normal; // vertex normal data (3*nnormal) - double* center; // face circumcenter data (3*nface) - float* texcoord; // vertex texcoord data (2*ntexcoord or NULL) - int* face; // face vertex indices (3*nface) - int* facenormal; // face normal indices (3*nface) - int* facetexcoord; // face texcoord indices (3*nface) - int* graph; // convex graph data + int nvert_; // number of vertices + int nnormal_; // number of normals + int ntexcoord_; // number of texcoords + int nface_; // number of faces + int szgraph_; // size of graph data in ints + float* vert_; // vertex data (3*nvert), relative to (pos, quat) + float* normal_; // vertex normal data (3*nnormal) + double* center_; // face circumcenter data (3*nface) + float* texcoord_; // vertex texcoord data (2*ntexcoord or NULL) + int* face_; // face vertex indices (3*nface) + int* facenormal_; // face normal indices (3*nface) + int* facetexcoord_; // face texcoord indices (3*nface) + int* graph_; // convex graph data - bool needhull; // needs convex hull for collisions + bool needhull_; // needs convex hull for collisions - mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy - std::vector face_aabb; // bounding boxes of all faces + mjCBoundingVolumeHierarchy tree_; // bounding volume hierarchy + std::vector face_aabb_; // bounding boxes of all faces }; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index c3d594c3..1b322c0e 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1155,7 +1155,6 @@ void mjXReader::Size(XMLElement* section, mjCModel* mod) { ReadAttrInt(section, "nuser_sensor", &mod->nuser_sensor); if (mod->nuser_sensor < -1) throw mjXError(section, "nuser_sensor must be >= -1"); - } @@ -1187,26 +1186,27 @@ void mjXReader::OneMesh(XMLElement* elem, mjCMesh* pmesh) { // read attributes ReadAttrTxt(elem, "name", pmesh->name); ReadAttrTxt(elem, "class", pmesh->classname); - ReadAttrTxt(elem, "content_type", pmesh->content_type); - ReadAttrTxt(elem, "file", pmesh->file); - ReadAttr(elem, "refpos", 3, pmesh->refpos, text); - ReadAttr(elem, "refquat", 4, pmesh->refquat, text); - ReadAttr(elem, "scale", 3, pmesh->scale, text); + pmesh->set_content_type(ReadAttrStr(elem, "content_type")); + pmesh->set_file(ReadAttrStr(elem, "file")); + pmesh->set_refpos(ReadAttrArr(elem, "refpos")); + pmesh->set_refquat(ReadAttrArr(elem, "refquat")); + pmesh->set_scale(ReadAttrArr(elem, "scale")); + if (MapValue(elem, "smoothnormal", &n, bool_map, 2)) { - pmesh->smoothnormal = (n==1); + pmesh->set_smoothnormal((n==1)); } // read user vertex data - if (ReadAttrTxt(elem, "vertex", text)) String2Vector(text, pmesh->uservert); + pmesh->set_uservert(ReadAttrVec(elem, "vertex")); // read user normal data - if (ReadAttrTxt(elem, "normal", text)) String2Vector(text, pmesh->usernormal); + pmesh->set_usernormal(ReadAttrVec(elem, "normal")); // read user texcoord data - if (ReadAttrTxt(elem, "texcoord", text)) String2Vector(text, pmesh->usertexcoord); + pmesh->set_usertexcoord(ReadAttrVec(elem, "texcoord")); // read user face data - if (ReadAttrTxt(elem, "face", text)) String2Vector(text, pmesh->userface); + pmesh->set_userface(ReadAttrVec(elem, "face")); GetXMLPos(elem, pmesh); } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index d8b687c9..cb5ffdc0 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -90,39 +90,39 @@ void mjXWriter::OneMesh(XMLElement* elem, mjCMesh* pmesh, mjCDef* def) { if (!writingdefaults) { WriteAttrTxt(elem, "name", pmesh->name); WriteAttrTxt(elem, "class", pmesh->classname); - WriteAttrTxt(elem, "content_type", pmesh->content_type); - WriteAttrTxt(elem, "file", pmesh->file); + WriteAttrTxt(elem, "content_type", pmesh->content_type()); + WriteAttrTxt(elem, "file", pmesh->file()); // write vertex data - if (!pmesh->uservert.empty()) { - Vector2String(text, pmesh->uservert); + if (!pmesh->uservert().empty()) { + Vector2String(text, pmesh->uservert()); WriteAttrTxt(elem, "vertex", text); } // write normal data - if (!pmesh->usernormal.empty()) { - Vector2String(text, pmesh->usernormal); + if (!pmesh->usernormal().empty()) { + Vector2String(text, pmesh->usernormal()); WriteAttrTxt(elem, "normal", text); } // write texcoord data - if (!pmesh->usertexcoord.empty()) { - Vector2String(text, pmesh->usertexcoord); + if (!pmesh->usertexcoord().empty()) { + Vector2String(text, pmesh->usertexcoord()); WriteAttrTxt(elem, "texcoord", text); } // write face data - if (!pmesh->userface.empty()) { - Vector2String(text, pmesh->userface); + if (!pmesh->userface().empty()) { + Vector2String(text, pmesh->userface()); WriteAttrTxt(elem, "face", text); } } // defaults and regular - WriteAttr(elem, "refpos", 3, pmesh->refpos, def->mesh.refpos); - WriteAttr(elem, "refquat", 4, pmesh->refquat, def->mesh.refquat); - WriteAttr(elem, "scale", 3, pmesh->scale, def->mesh.scale); - WriteAttrKey(elem, "smoothnormal", bool_map, 2, pmesh->smoothnormal, def->mesh.smoothnormal); + WriteAttr(elem, "refpos", 3, pmesh->refpos(), def->mesh.refpos()); + WriteAttr(elem, "refquat", 4, pmesh->refquat(), def->mesh.refquat()); + WriteAttr(elem, "scale", 3, pmesh->scale(), def->mesh.scale()); + WriteAttrKey(elem, "smoothnormal", bool_map, 2, pmesh->smoothnormal(), def->mesh.smoothnormal()); } diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index 9f83b91c..01b4d6a7 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -542,10 +542,11 @@ mjCGeom* mjXURDF::Geom(XMLElement* geom_elem, mjCBody* pbody, bool collision) { // mesh else if ((temp = FindSubElem(elem, "mesh"))) { // set geom type and read mesh attributes - double meshscale[3] = {1, 1, 1}; pgeom->type = mjGEOM_MESH; - ReadAttrTxt(temp, "filename", meshfile, true); - ReadAttr(temp, "scale", 3, meshscale, text); + meshfile = ReadAttrStr(temp, "filename", true).value(); + std::array default_meshscale = {1, 1, 1}; + std::array meshscale = ReadAttrArr(temp, "scale") + .value_or(default_meshscale); // strip file name if necessary if (model->strippath) { @@ -565,18 +566,18 @@ mjCGeom* mjXURDF::Geom(XMLElement* geom_elem, mjCBody* pbody, bool collision) { } // exists with different scale: append name with '1', create - else if (pmesh->scale[0]!=meshscale[0] || - pmesh->scale[1]!=meshscale[1] || - pmesh->scale[2]!=meshscale[2]) { + else if (pmesh->scale()[0]!=meshscale[0] || + pmesh->scale()[1]!=meshscale[1] || + pmesh->scale()[2]!=meshscale[2]) { pmesh = model->AddMesh(); meshname = meshname + "1"; } // set fields - pmesh->file = meshfile; + pmesh->set_file(meshfile); pmesh->name = meshname; pgeom->mesh = meshname; - mjuu_copyvec(pmesh->scale, meshscale, 3); + pmesh->set_scale(meshscale); } else { diff --git a/src/xml/xml_util.cc b/src/xml/xml_util.cc index d47f9dba..d3965945 100644 --- a/src/xml/xml_util.cc +++ b/src/xml/xml_util.cc @@ -895,7 +895,7 @@ static int Round(double x) { // write attribute template -void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, T* data, const T* def) { +void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, const T* data, const T* def) { // make sure all are defined if constexpr (std::is_floating_point_v) { for (int i=0; i attribute, default = zero array diff --git a/src/xml/xml_util.h b/src/xml/xml_util.h index 10b6f2db..68c30d61 100644 --- a/src/xml/xml_util.h +++ b/src/xml/xml_util.h @@ -179,7 +179,7 @@ class mjXUtil { // write attribute- any type template - static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, T* data, + static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, const T* data, const T* def = 0); // write vector attribute, with and without default