diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 87cb094f..a0abee9f 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -877,6 +877,33 @@ void mjCMesh::LoadMSH(int default_provider) { } +void mjCMesh::ComputeVolume(double CoM[3], mjtMeshType type, + const double facecen[3], bool exactmeshinertia) { + double nrm[3]; + double cen[3]; + GetVolumeRef(type) = 0; + mjuu_zerovec(CoM, 3); + for (int i=0; iexactmeshinertia; if (type==mjVOLUME_MESH) { // translate @@ -995,27 +1023,16 @@ void mjCMesh::Process() { } // compute CoM and volume from pyramid volumes - GetVolumeRef(type) = 0; - for (int i=0; iexactmeshinertia); - // compute and add volume - const double vec[3] = {cen[0]-facecen[0], cen[1]-facecen[1], cen[2]-facecen[2]}; - double vol = type==mjSHELL_MESH ? a : mjuu_dot3(vec, nrm) * a / 3; - - // if legacy computation requested, then always positive - if (!model->exactmeshinertia && type==mjVOLUME_MESH) { - vol = fabs(vol); - } - - // add pyramid com - GetVolumeRef(type) += vol; - for (int j=0; j<3; j++) { - CoM[j] += vol*(cen[j]*3.0/4.0 + facecen[j]/4.0); - } + // perform computation again if volume is negative + if (GetVolumeRef(type) < mjMINVAL && exactmeshinertia) { + mju_warning("Malformed mesh %s, computing mesh inertia from convex hull", name.c_str()); + exactmeshinertia = false; + ComputeVolume(CoM, type, facecen, exactmeshinertia); } + // require positive volume if (GetVolumeRef(type) < mjMINVAL) { validvolume = GetVolumeRef(type) < 0 ? -1 : 0; @@ -1051,7 +1068,7 @@ void mjCMesh::Process() { double vol = type==mjSHELL_MESH ? a : mjuu_dot3(cen, nrm) * a / 3; // if legacy computation requested, then always positive - if (!model->exactmeshinertia && type==mjVOLUME_MESH) { + if (!exactmeshinertia && type==mjVOLUME_MESH) { vol = fabs(vol); } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 43c20fdb..db8595f0 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -523,6 +523,10 @@ class mjCMesh: public mjCBase { void RemoveRepeated(void); // remove repeated vertices void CheckMesh(void); // check if the mesh is valid + // compute the volume and center-of-mass of the mesh given the face center + void ComputeVolume(double CoM[3], mjtMeshType type, const double facecen[3], + 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 diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index e511a795..a29c9c46 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -464,7 +464,7 @@ TEST_F(MjCMeshTest, VolumeTooSmall) { EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small")); } -TEST_F(MjCMeshTest, VolumeNegative) { +TEST_F(MjCMeshTest, VolumeNegativeDefaultsLegacy) { static constexpr char xml[] = R"( @@ -482,8 +482,12 @@ TEST_F(MjCMeshTest, VolumeNegative) { )"; std::array error; mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, testing::IsNull()); - EXPECT_THAT(error.data(), HasSubstr("mesh volume is negative")); + EXPECT_THAT(model, testing::NotNull()); + EXPECT_LE(mju_abs(model->geom_size[0]), 1); + EXPECT_LE(mju_abs(model->geom_size[1]), 1); + EXPECT_LE(mju_abs(model->geom_size[2]), 1); + EXPECT_THAT(error.data(), HasSubstr("Malformed")); + mj_deleteModel(model); } TEST_F(MjCMeshTest, VolumeTooSmallAllowedWorld) {