From 3487086a6ba94b9d53eeb2a1b27968bab382e181 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 25 Jun 2024 23:52:28 +0000 Subject: [PATCH] Support visualizing mesh collisions with convex decomposition (#2352) Signed-off-by: Ian Chen --- src/Conversions.cc | 36 ++++++++++++++++ src/Conversions_TEST.cc | 19 +++++++++ src/Util.cc | 42 +++++++++++++++---- .../VisualizationCapabilities.cc | 19 +++++++-- 4 files changed, 106 insertions(+), 10 deletions(-) diff --git a/src/Conversions.cc b/src/Conversions.cc index e10e0d1184..8c72d403e0 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -220,6 +220,24 @@ msgs::Geometry gz::sim::convert(const sdf::Geometry &_in) meshMsg->set_filename(asFullPath(meshSdf->Uri(), meshSdf->FilePath())); meshMsg->set_submesh(meshSdf->Submesh()); meshMsg->set_center_submesh(meshSdf->CenterSubmesh()); + + if (!meshSdf->OptimizationStr().empty()) + { + auto header = out.mutable_header()->add_data(); + header->set_key("optimization"); + header->add_value(meshSdf->OptimizationStr()); + } + if (meshSdf->ConvexDecomposition()) + { + auto header = out.mutable_header()->add_data(); + header->set_key("max_convex_hulls"); + header->add_value(std::to_string( + meshSdf->ConvexDecomposition()->MaxConvexHulls())); + header = out.mutable_header()->add_data(); + header->set_key("voxel_resolution"); + header->add_value(std::to_string( + meshSdf->ConvexDecomposition()->VoxelResolution())); + } } else if (_in.Type() == sdf::GeometryType::HEIGHTMAP && _in.HeightmapShape()) { @@ -359,6 +377,24 @@ sdf::Geometry gz::sim::convert(const msgs::Geometry &_in) meshShape.SetSubmesh(_in.mesh().submesh()); meshShape.SetCenterSubmesh(_in.mesh().center_submesh()); + sdf::ConvexDecomposition convexDecomp; + for (int i = 0; i < _in.header().data_size(); ++i) + { + auto data = _in.header().data(i); + if (data.key() == "optimization" && data.value_size() > 0) + { + meshShape.SetOptimization(data.value(0)); + } + if (data.key() == "max_convex_hulls" && data.value_size() > 0) + { + convexDecomp.SetMaxConvexHulls(std::stoul(data.value(0))); + } + if (data.key() == "voxel_resolution" && data.value_size() > 0) + { + convexDecomp.SetVoxelResolution(std::stoul(data.value(0))); + } + } + meshShape.SetConvexDecomposition(convexDecomp); out.SetMeshShape(meshShape); } else if (_in.type() == msgs::Geometry::HEIGHTMAP && _in.has_heightmap()) diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 08ac3e1afd..e14cf6b21b 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -463,6 +463,11 @@ TEST(Conversions, GeometryMesh) meshShape.SetUri("file://watermelon"); meshShape.SetSubmesh("grape"); meshShape.SetCenterSubmesh(true); + meshShape.SetOptimization("convex_decomposition"); + sdf::ConvexDecomposition convexDecomp; + convexDecomp.SetMaxConvexHulls(4); + convexDecomp.SetVoxelResolution(10000); + meshShape.SetConvexDecomposition(convexDecomp); geometry.SetMeshShape(meshShape); auto geometryMsg = convert(geometry); @@ -473,6 +478,15 @@ TEST(Conversions, GeometryMesh) EXPECT_EQ("file://watermelon", geometryMsg.mesh().filename()); EXPECT_EQ("grape", geometryMsg.mesh().submesh()); EXPECT_TRUE(geometryMsg.mesh().center_submesh()); + auto header = geometryMsg.header().data(0); + EXPECT_EQ("optimization", header.key()); + EXPECT_EQ("convex_decomposition", header.value(0)); + header = geometryMsg.header().data(1); + EXPECT_EQ("max_convex_hulls", header.key()); + EXPECT_EQ("4", header.value(0)); + header = geometryMsg.header().data(2); + EXPECT_EQ("voxel_resolution", header.key()); + EXPECT_EQ("10000", header.value(0)); auto newGeometry = convert(geometryMsg); EXPECT_EQ(sdf::GeometryType::MESH, newGeometry.Type()); @@ -481,6 +495,11 @@ TEST(Conversions, GeometryMesh) EXPECT_EQ("file://watermelon", newGeometry.MeshShape()->Uri()); EXPECT_EQ("grape", newGeometry.MeshShape()->Submesh()); EXPECT_TRUE(newGeometry.MeshShape()->CenterSubmesh()); + EXPECT_EQ("convex_decomposition", newGeometry.MeshShape()->OptimizationStr()); + auto newConvexDecomp = newGeometry.MeshShape()->ConvexDecomposition(); + ASSERT_NE(nullptr, newConvexDecomp); + EXPECT_EQ(4, newConvexDecomp->MaxConvexHulls()); + EXPECT_EQ(10000, newConvexDecomp->VoxelResolution()); } ///////////////////////////////////////////////// diff --git a/src/Util.cc b/src/Util.cc index e9cb2406f0..d5eb58bc65 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -891,6 +891,7 @@ const common::Mesh *optimizeMesh(const sdf::Mesh &_meshSdf, auto &meshManager = *common::MeshManager::Instance(); std::size_t maxConvexHulls = 16u; + std::size_t voxelResolution = 200000u; if (_meshSdf.Optimization() == sdf::MeshOptimization::CONVEX_HULL) { /// create 1 convex hull for the whole submesh @@ -898,25 +899,52 @@ const common::Mesh *optimizeMesh(const sdf::Mesh &_meshSdf, } else if (_meshSdf.ConvexDecomposition()) { - // limit max number of convex hulls to generate + // set convex decomposition params: max number of convex hulls + // and voxel resolution maxConvexHulls = _meshSdf.ConvexDecomposition()->MaxConvexHulls(); + voxelResolution = _meshSdf.ConvexDecomposition()->VoxelResolution(); } // Check if MeshManager contains the decomposed mesh already. If not // add it to the MeshManager so we do not need to decompose it again. const std::string convexMeshName = - _mesh.Name() + "_CONVEX_" + std::to_string(maxConvexHulls); + _mesh.Name() + "_" + _meshSdf.Submesh() + "_CONVEX_" + + std::to_string(maxConvexHulls) + "_" + std::to_string(voxelResolution); auto *optimizedMesh = meshManager.MeshByName(convexMeshName); if (!optimizedMesh) { - // Merge meshes before convex decomposition - auto mergedMesh = gz::common::MeshManager::MergeSubMeshes(_mesh); - if (mergedMesh && mergedMesh->SubMeshCount() == 1u) + std::unique_ptr meshToDecompose = + std::make_unique(); + // check if a particular submesh is requested + if (!_meshSdf.Submesh().empty()) + { + for (unsigned int submeshIdx = 0; + submeshIdx < _mesh.SubMeshCount(); + ++submeshIdx) + { + auto submesh = _mesh.SubMeshByIndex(submeshIdx).lock(); + if (submesh->Name() == _meshSdf.Submesh()) + { + if (_meshSdf.CenterSubmesh()) + submesh->Center(math::Vector3d::Zero); + meshToDecompose->AddSubMesh(*submesh.get()); + break; + } + } + } + else + { + // Merge meshes before convex decomposition + meshToDecompose = + gz::common::MeshManager::MergeSubMeshes(_mesh); + } + + if (meshToDecompose && meshToDecompose->SubMeshCount() == 1u) { // Decompose and add mesh to MeshManager - auto mergedSubmesh = mergedMesh->SubMeshByIndex(0u).lock(); + auto mergedSubmesh = meshToDecompose->SubMeshByIndex(0u).lock(); std::vector decomposed = gz::common::MeshManager::ConvexDecomposition( - *mergedSubmesh.get(), maxConvexHulls); + *mergedSubmesh.get(), maxConvexHulls, voxelResolution); gzdbg << "Optimizing mesh (" << _meshSdf.OptimizationStr() << "): " << _mesh.Name() << std::endl; // Create decomposed mesh and add it to MeshManager diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc index 2694553d54..92ceedbfbb 100644 --- a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -39,6 +40,7 @@ #include #include #include +#include #include #include @@ -1247,14 +1249,25 @@ rendering::GeometryPtr VisualizationCapabilitiesPrivate::CreateGeometry( // Assume absolute path to mesh file descriptor.meshName = fullPath; - descriptor.subMeshName = _geom.MeshShape()->Submesh(); - descriptor.centerSubMesh = _geom.MeshShape()->CenterSubmesh(); - gz::common::MeshManager *meshManager = gz::common::MeshManager::Instance(); descriptor.mesh = meshManager->Load(descriptor.meshName); if (descriptor.mesh) { + if (_geom.MeshShape()->Optimization() != sdf::MeshOptimization::NONE) + { + const common::Mesh *optimizedMesh = + optimizeMesh(*_geom.MeshShape(), *descriptor.mesh); + if (optimizedMesh) + { + descriptor.mesh = optimizedMesh; + // if submesh is requested, it should be handled in the optimizeMesh + // function so we do not need need to pass these flags to + // gz-rendering + descriptor.subMeshName = ""; + descriptor.centerSubMesh = false; + } + } geom = this->scene->CreateMesh(descriptor); } else