diff --git a/doc/changelog.rst b/doc/changelog.rst index 57f94107..d2d2e09b 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -18,6 +18,9 @@ General Bug fixes ^^^^^^^^^ +- Fixed a bug that for :at:`shellinertia` equal to ``true`` caused the mesh orientation to be overwritten by the + principal components of the shell inertia, while the vertex coordinates are rotated using the volumetric inertia. + Now the volumetric inertia orientation is used also in the shell case. - Fixed misalignment bug in mesh-to-primitive fitting when using the bounding box fitting option :at:`fitaabb`. - The ``launch_repl`` functionality in the Python viewer is fixed. diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index f2191b43..0e50c231 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -1152,7 +1152,7 @@ void mjCMesh::Process() { // copy quat for (j=0; j<4; j++) { - GetQuatPtr(type)[j] = quattmp[j]; + GetQuatPtr(type)[j] = type == mjVOLUME_MESH ? quattmp[j] : GetQuatPtr(mjVOLUME_MESH)[j]; } // rotate vertices and normals into axis-aligned frame diff --git a/test/user/testdata/torus_shell.xml b/test/user/testdata/torus_shell.xml new file mode 100644 index 00000000..b31a6039 --- /dev/null +++ b/test/user/testdata/torus_shell.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index b3b12611..b91be472 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -38,6 +38,8 @@ static const char* const kCubePath = "user/testdata/cube.xml"; static const char* const kTorusPath = "user/testdata/torus.xml"; +static const char* const kTorusShellPath = + "user/testdata/torus_shell.xml"; static const char* const kConvexInertiaPath = "user/testdata/inertia_convex.xml"; static const char* const kConcaveInertiaPath = @@ -352,6 +354,21 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedNegligibleArea) { mj_deleteModel(model); } +TEST_F(MjCMeshTest, ShellUsesVolumeFrame) { + const std::string xml_path_v = GetTestDataFilePath(kTorusPath); + const std::string xml_path_s = GetTestDataFilePath(kTorusShellPath); + std::array error; + mjModel* mv = mj_loadXML(xml_path_v.c_str(), 0, error.data(), error.size()); + mjModel* ms = mj_loadXML(xml_path_s.c_str(), 0, error.data(), error.size()); + mjtNum tolerance = std::numeric_limits::epsilon(); + EXPECT_NEAR(mv->geom_quat[0], ms->geom_quat[0], tolerance); + EXPECT_NEAR(mv->geom_quat[1], ms->geom_quat[1], tolerance); + EXPECT_NEAR(mv->geom_quat[2], ms->geom_quat[2], tolerance); + EXPECT_NEAR(mv->geom_quat[3], ms->geom_quat[3], tolerance); + mj_deleteModel(mv); + mj_deleteModel(ms); +} + TEST_F(MjCMeshTest, AreaTooSmall) { static constexpr char xml[] = R"(