From 0e05e7ce527814aeacd6571ac77246a9c76f5a7b Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 15 Sep 2022 07:06:22 -0700 Subject: [PATCH] Do not raise an error for flipped triangles if their area contribution is negligible. PiperOrigin-RevId: 474552367 Change-Id: I9727a74e50a9bab9b04bb0de0b64e11587ed36f1 --- src/user/user_mesh.cc | 136 +++++++++++++++++++++--------------- test/user/user_mesh_test.cc | 22 ++++++ 2 files changed, 103 insertions(+), 55 deletions(-) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 81f11723..a5991241 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -48,6 +48,38 @@ extern "C" { using std::string; using std::vector; +// compute triangle area, surface normal, center +static mjtNum _triangle(mjtNum* normal, mjtNum* center, + const float* v1, const float* v2, const float* v3) { + // center + if (center) { + for (int i=0; i<3; i++) { + center[i] = (v1[i] + v2[i] + v3[i])/3; + } + } + + // normal = (v2-v1) cross (v3-v1) + double b[3] = { v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2] }; + double c[3] = { v3[0]-v1[0], v3[1]-v1[1], v3[2]-v1[2] }; + mju_cross(normal, b, c); + + // get length + double len = mju_norm3(normal); + + // ignore small faces + if (len=nvert || vertex_index < 0) { + throw mjCError(this, "found index in userface that exceeds uservert size."); + } + } + // 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)); + } else { + mju_warning("Degenerate triangle found, its orientation will not be checked."); + } } } } @@ -554,6 +598,7 @@ void mjCMesh::LoadOBJ(const mjVFS* vfs) { bool has_normals = !attrib.normals.empty(); bool has_texcoords = !attrib.texcoords.empty(); bool righthand = (scale[0]*scale[1]*scale[2] > 0); + // iterate over mesh faces int index_in_mesh_indices = 0; for (int face = 0; face < mesh.num_face_vertices.size(); face++) { @@ -562,6 +607,7 @@ void mjCMesh::LoadOBJ(const mjVFS* vfs) { this, "only tri or quad meshes are supported for OBJ (file '%s')", filename.c_str()); } + // add face std::vector> faces; tinyobj::index_t v0 = mesh.indices[index_in_mesh_indices]; @@ -569,18 +615,12 @@ void mjCMesh::LoadOBJ(const mjVFS* vfs) { tinyobj::index_t v2 = mesh.indices[index_in_mesh_indices+2]; std::array face1 = {v0, v1, v2}; faces.push_back(face1); - // add edges - useredge.push_back(std::pair(v0.vertex_index, v1.vertex_index)); - useredge.push_back(std::pair(v1.vertex_index, v2.vertex_index)); - useredge.push_back(std::pair(v2.vertex_index, v0.vertex_index)); + // handle quad: add second triangle with 4th vertex if (mesh.num_face_vertices[face] == 4) { tinyobj::index_t v3 = mesh.indices[index_in_mesh_indices+3]; std::array face2 = {v0, v2, v3}; faces.push_back(face2); - useredge.push_back(std::pair(v0.vertex_index, v2.vertex_index)); - useredge.push_back(std::pair(v2.vertex_index, v3.vertex_index)); - useredge.push_back(std::pair(v3.vertex_index, v0.vertex_index)); } for (const auto& face_indices : faces) { int index_of_first_vertex = uservert.size()/3; @@ -590,6 +630,7 @@ void mjCMesh::LoadOBJ(const mjVFS* vfs) { uservert.end(), attrib.vertices.begin() + 3*tinyobj_index.vertex_index, attrib.vertices.begin() + 3*tinyobj_index.vertex_index + 3); + // for each vertex, add its normal if (has_normals) { usernormal.insert( @@ -597,6 +638,7 @@ void mjCMesh::LoadOBJ(const mjVFS* vfs) { attrib.normals.begin() + 3*tinyobj_index.normal_index, attrib.normals.begin() + 3*tinyobj_index.normal_index + 3); } + // for each vertex, add two entries to usertexcoord if (has_texcoords) { usertexcoord.push_back( @@ -605,14 +647,33 @@ void mjCMesh::LoadOBJ(const mjVFS* vfs) { 1-attrib.texcoords[2*tinyobj_index.texcoord_index + 1]); } } - // add vertex indices (in uservert) to userface - userface.push_back(index_of_first_vertex); - if (righthand) { - userface.push_back(index_of_first_vertex+1); - userface.push_back(index_of_first_vertex+2); + int i0 = index_of_first_vertex; + int i1 = index_of_first_vertex+1; + int i2 = index_of_first_vertex+2; + + // add edges + const float *v0 = uservert.data() + 3*i0; + const float *v1 = uservert.data() + 3*i1; + const float *v2 = uservert.data() + 3*i2; + mjtNum normal[3]; + + // only consider edges if the face contribution is significant + if (_triangle(normal, nullptr, v0, v1, v2)>sqrt(mjMINVAL)) { + useredge.push_back(std::pair(face_indices[0].vertex_index, face_indices[1].vertex_index)); + useredge.push_back(std::pair(face_indices[1].vertex_index, face_indices[2].vertex_index)); + useredge.push_back(std::pair(face_indices[2].vertex_index, face_indices[0].vertex_index)); } else { - userface.push_back(index_of_first_vertex+2); - userface.push_back(index_of_first_vertex+1); + mju_warning("Degenerate triangle found, its orientation will not be checked."); + } + + // add vertex indices (in uservert) to userface + userface.push_back(i0); + if (righthand) { + userface.push_back(i1); + userface.push_back(i2); + } else { + userface.push_back(i2); + userface.push_back(i1); } } index_in_mesh_indices += mesh.num_face_vertices[face]; @@ -859,41 +920,6 @@ void mjCMesh::LoadMSH(const mjVFS* vfs) { -// compute triangle area, surface normal, center -static double _areaNrmCen(double* normal, double* center, - const float* v1, const float* v2, const float* v3) { - // center - if (center) { - for (int i=0; i<3; i++) { - center[i] = (v1[i] + v2[i] + v3[i])/3; - } - } - - // normal = (v2-v1) cross (v3-v1) - double b[3] = { v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2] }; - double c[3] = { v3[0]-v1[0], v3[1]-v1[1], v3[2]-v1[2] }; - normal[0] = b[1]*c[2] - b[2]*c[1]; - normal[1] = b[2]*c[0] - b[0]*c[2]; - normal[2] = b[0]*c[1] - b[1]*c[0]; - - // get length - double len = sqrt(normal[0]*normal[0] + normal[1]*normal[1] + normal[2]*normal[2]); - - // ignore small faces - if (len + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::NotNull()); + CheckTetrahedronWasRescaled(model); + mj_deleteModel(model); +} + TEST_F(MujocoTest, AreaTooSmall) { static constexpr char xml[] = R"(