diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 7f15a9b8..7997c8de 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -204,8 +204,9 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) { if (pingridrange.size()%(2*dflex->dim)) { return comperr(error, "Pin grid range number of must be multiple of 2*dim", error_sz); } - if (type != mjFCOMPTYPE_GRID && !(pingrid.empty() && pingridrange.empty())) { - return comperr(error, "Pin grid(range) can only be used with grid type", error_sz); + if (type != mjFCOMPTYPE_GRID && !(pingrid.empty() && pingridrange.empty()) && + doftype != mjFCOMPDOF_TRILINEAR && doftype != mjFCOMPDOF_QUADRATIC) { + return comperr(error, "Pin grid(range) can only be used with grid or interpolated", error_sz); } if (dflex->dim == 1 && !(pingrid.empty() && pingridrange.empty())) { return comperr(error, "Pin grid(range) cannot be used with dim=1", error_sz); @@ -267,7 +268,13 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) { } // construct pinned array - pinned = vector(npnt, rigid); + int nnode = 0; + if (doftype == mjFCOMPDOF_TRILINEAR) { + nnode = 8; + } else if (doftype == mjFCOMPDOF_QUADRATIC) { + nnode = 27; + } + pinned = vector(std::max(npnt, nnode), rigid); // handle pins if user did not specify rigid if (!rigid) { @@ -299,8 +306,14 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) { // process pingrid for (int i=0; i < (int)pingrid.size(); i+=dflex->dim) { // check range + int count_check[3] = {count[0], count[1], count[2]}; + if (type != mjFCOMPTYPE_GRID && (doftype == mjFCOMPDOF_TRILINEAR || + doftype == mjFCOMPDOF_QUADRATIC)) { + int dim = (doftype == mjFCOMPDOF_TRILINEAR) ? 2 : 3; + count_check[0] = count_check[1] = count_check[2] = dim; + } for (int k=0; k < dflex->dim; k++) { - if (pingrid[i+k] < 0 || pingrid[i+k] >= count[k]) { + if (pingrid[i+k] < 0 || pingrid[i+k] >= count_check[k]) { return comperr(error, "pingrid out of range", error_sz); } } @@ -392,6 +405,35 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) { for (int i=0; i < (int)element.size(); i++) { element[i] += reindex[element[i]]; } + + // compact point, texcoord, pinned arrays + int new_npnt = 0; + for (int i=0; i < npnt; i++) { + if (used[i]) { + point[3*new_npnt+0] = point[3*i+0]; + point[3*new_npnt+1] = point[3*i+1]; + point[3*new_npnt+2] = point[3*i+2]; + + if (!texcoord.empty()) { + texcoord[2*new_npnt+0] = texcoord[2*i+0]; + texcoord[2*new_npnt+1] = texcoord[2*i+1]; + } + + pinned[new_npnt] = pinned[i]; + new_npnt++; + } + } + + // resize arrays + point.resize(3*new_npnt); + if (!texcoord.empty()) { + texcoord.resize(2*new_npnt); + } + pinned.resize(std::max(new_npnt, nnode)); + used.assign(new_npnt, true); + + // update count + npnt = new_npnt; } } diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 000c298b..825f8901 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4137,6 +4137,8 @@ void mjCFlex::Compile(const mjVFS* vfs) { nvert = (int)vert_.size()/3; if (vertbody_.size() == 1) { rigid = true; + } else if (vertbody_.size() != nvert) { + throw mjCError(this, "vertbody size must be 1 or nvert"); } } if (nvert < dim+1) { diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index 492c4170..8db36823 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -786,5 +786,55 @@ TEST_F(UserFlexTest, LoadMSHASCII_dim_missing_in_xml) { mj_deleteModel(m); } +TEST_F(UserFlexTest, TrilinearUnusedVertices_Crash) { + // This XML defines 5 points but only uses 4 in the element. + // The last point (2 2 2) is unused. + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, testing::NotNull()) << error.data(); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, MeshNodePinning) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, testing::NotNull()) << error.data(); + + // Verify that node 0 (corner) is pinned + // Trilinear 3D has 8 nodes. + // If 0 are pinned, we get 8 bodies. + // If 1 is pinned, we get 7 bodies (created) + 1 existing (the parent). + // m->nbody should reflect this. Flex bodies are added to the model. + // Model has 1 world body + flex bodies. + EXPECT_EQ(m->nbody, 1 + 7); // 1 world + 7 flex nodes (1 pinned) + + mj_deleteModel(m); +} + } // namespace } // namespace mujoco