diff --git a/cmake/MujocoDependencies.cmake b/cmake/MujocoDependencies.cmake index 699bbb15..d2404bc1 100644 --- a/cmake/MujocoDependencies.cmake +++ b/cmake/MujocoDependencies.cmake @@ -58,11 +58,6 @@ set(MUJOCO_DEP_VERSION_benchmark CACHE STRING "Version of `benchmark` to be fetched." ) -set(MUJOCO_DEP_VERSION_TriangleMeshDistance - 2cb643de1436e1ba8e2be49b07ec5491ac604457 - CACHE STRING "Version of `TriangleMeshDistance` to be fetched." -) - mark_as_advanced(MUJOCO_DEP_VERSION_lodepng) mark_as_advanced(MUJOCO_DEP_VERSION_MarchingCubeCpp) mark_as_advanced(MUJOCO_DEP_VERSION_tinyxml2) @@ -73,7 +68,6 @@ mark_as_advanced(MUJOCO_DEP_VERSION_Eigen3) mark_as_advanced(MUJOCO_DEP_VERSION_abseil) mark_as_advanced(MUJOCO_DEP_VERSION_gtest) mark_as_advanced(MUJOCO_DEP_VERSION_benchmark) -mark_as_advanced(MUJOCO_DEP_VERSION_TriangleMeshDistance) include(FetchContent) include(FindOrFetch) @@ -202,30 +196,6 @@ if(CMAKE_POLICY_VERSION_MINIMUM_LOCALLY_DEFINED) unset(CMAKE_POLICY_VERSION_MINIMUM_LOCALLY_DEFINED) endif() -if(NOT TARGET trianglemeshdistance) - FetchContent_Declare( - trianglemeshdistance - GIT_REPOSITORY https://github.com/InteractiveComputerGraphics/TriangleMeshDistance.git - GIT_TAG ${MUJOCO_DEP_VERSION_TriangleMeshDistance} - ) - - FetchContent_GetProperties(trianglemeshdistance) - if(NOT trianglemeshdistance_POPULATED) - FetchContent_Populate(trianglemeshdistance) - # Patch the source code to silence a warning/error related to a loop variable creating a copy. - # Since this is a header only library this fix is less intrusive than disabling the warning for - # any target including the header. - set(TMD_HEADER ${trianglemeshdistance_SOURCE_DIR}/TriangleMeshDistance/include/tmd/TriangleMeshDistance.h) - file(READ ${TMD_HEADER} TMD_CONTENT) - string(REPLACE - "for (const auto edge_count : edges_count) {" - "for (const auto& edge_count : edges_count) {" - TMD_CONTENT "${TMD_CONTENT}") - file(WRITE ${TMD_HEADER} "${TMD_CONTENT}") - include_directories(${trianglemeshdistance_SOURCE_DIR}) - endif() -endif() - set(ENABLE_DOUBLE_PRECISION ON) set(CCD_HIDE_ALL_SYMBOLS ON) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index c1e1a027..812eb0df 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -779,7 +779,7 @@ void mjCMesh::TryCompile(const mjVFS* vfs) { // compute sdf coefficients if (!plugin.active) { - octree_.ComputeSdfCoeffs(vert_.data(), nvert(), face_.data(), nface()); + octree_.ComputeSdfCoeffs(vert_.data(), nvert(), face_.data(), nface(), tree_); } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 06045081..a5c77e9c 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -25,10 +25,12 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include @@ -47,7 +49,6 @@ #include "user/user_model.h" #include "user/user_resource.h" #include "user/user_util.h" -#include namespace { namespace mju = ::mujoco::util; @@ -375,6 +376,7 @@ void mjCBoundingVolumeHierarchy::RemoveInactiveVolumes(int nmax) { bvleaf_.erase(bvleaf_.begin() + nmax, bvleaf_.end()); } + const mjCBoundingVolume* mjCBoundingVolumeHierarchy::AddBoundingVolume(int id, int contype, int conaffinity, const double* pos, const double* quat, @@ -553,6 +555,7 @@ void mjCOctree::CopyLevel(int* level) const { } } + void mjCOctree::CopyChild(int* child) const { for (int i = 0; i < node_.size(); ++i) { for (int j = 0; j < 8; ++j) { @@ -561,6 +564,7 @@ void mjCOctree::CopyChild(int* child) const { } } + void mjCOctree::CopyAabb(mjtNum* aabb) const { for (int i = 0; i < node_.size(); ++i) { aabb[i * 6 + 0] = (node_[i].aamm[0] + node_[i].aamm[3]) / 2; @@ -572,6 +576,7 @@ void mjCOctree::CopyAabb(mjtNum* aabb) const { } } + void mjCOctree::CopyCoeff(mjtNum* coeff) const { for (int i = 0; i < node_.size(); ++i) { for (int j = 0; j < 8; ++j) { @@ -580,6 +585,7 @@ void mjCOctree::CopyCoeff(mjtNum* coeff) const { } } + void mjCOctree::SetFace(const std::vector& vert, const std::vector& face) { for (int i = 0; i < face.size(); i += 3) { std::array v0 = {vert[3*face[i+0]], vert[3*face[i+0]+1], vert[3*face[i+0]+2]}; @@ -622,14 +628,207 @@ void mjCOctree::CreateOctree(const double aamm[6]) { } -// compute SDF coefficients at octree vertices using triangle mesh distance -void mjCOctree::ComputeSdfCoeffs(const double* vert, int nvert, - const int* face, int nface) { - tmd::TriangleMeshDistance sdf(vert, static_cast(nvert), - face, static_cast(nface)); +namespace { - std::vector coeffs(NumVerts()); - std::vector processed(NumVerts(), false); +double pointBoxDistSq(const double* p, const mjtNum* aabb) { + double dist_sq = 0; + for (int i = 0; i < 3; ++i) { + double lo = aabb[i] - aabb[i + 3]; + double hi = aabb[i] + aabb[i + 3]; + if (p[i] < lo) { + dist_sq += (lo - p[i]) * (lo - p[i]); + } else if (p[i] > hi) { + dist_sq += (p[i] - hi) * (p[i] - hi); + } + } + return dist_sq; +} + + +// compute squared distance between point p and triangle (v0, v1, v2), +// and return barycentric coordinates (u,v) of the closest point +double pointTriDistSqWithUV(const double* p, const double* v0, const double* v1, + const double* v2, double& out_u, double& out_v) { + double ab[3] = {v1[0] - v0[0], v1[1] - v0[1], v1[2] - v0[2]}; + double ac[3] = {v2[0] - v0[0], v2[1] - v0[1], v2[2] - v0[2]}; + double ap[3] = {p[0] - v0[0], p[1] - v0[1], p[2] - v0[2]}; + + // the closest point on the triangle is determined by partitioning space into Voronoi regions + double d1 = ab[0]*ap[0] + ab[1]*ap[1] + ab[2]*ap[2]; + double d2 = ac[0]*ap[0] + ac[1]*ap[1] + ac[2]*ap[2]; + + // region A (vertex v0) + if (d1 <= 0 && d2 <= 0) { + out_u = 0; out_v = 0; + return ap[0]*ap[0] + ap[1]*ap[1] + ap[2]*ap[2]; + } + + double bp[3] = {p[0] - v1[0], p[1] - v1[1], p[2] - v1[2]}; + double d3 = ab[0]*bp[0] + ab[1]*bp[1] + ab[2]*bp[2]; + double d4 = ac[0]*bp[0] + ac[1]*bp[1] + ac[2]*bp[2]; + + // region B (vertex v1) + if (d3 >= 0 && d4 <= d3) { + out_u = 1; out_v = 0; + return bp[0]*bp[0] + bp[1]*bp[1] + bp[2]*bp[2]; + } + + // region AB (edge v0-v1) + double vc = d1*d4 - d3*d2; + if (vc <= 0 && d1 >= 0 && d3 <= 0) { + double u = d1 / (d1 - d3); + out_u = u; out_v = 0; + double closest[3] = {v0[0] + u*ab[0], v0[1] + u*ab[1], v0[2] + u*ab[2]}; + return (p[0]-closest[0])*(p[0]-closest[0]) + + (p[1]-closest[1])*(p[1]-closest[1]) + + (p[2]-closest[2])*(p[2]-closest[2]); + } + + double cp[3] = {p[0] - v2[0], p[1] - v2[1], p[2] - v2[2]}; + double d5 = ab[0]*cp[0] + ab[1]*cp[1] + ab[2]*cp[2]; + double d6 = ac[0]*cp[0] + ac[1]*cp[1] + ac[2]*cp[2]; + + // region C (vertex v2) + if (d6 >= 0 && d5 <= d6) { + out_u = 0; out_v = 1; + return cp[0]*cp[0] + cp[1]*cp[1] + cp[2]*cp[2]; + } + + // region AC (edge v0-v2) + double vb = d5*d2 - d1*d6; + if (vb <= 0 && d2 >= 0 && d6 <= 0) { + double v = d2 / (d2 - d6); + out_u = 0; out_v = v; + double closest[3] = {v0[0] + v*ac[0], v0[1] + v*ac[1], v0[2] + v*ac[2]}; + return (p[0]-closest[0])*(p[0]-closest[0]) + + (p[1]-closest[1])*(p[1]-closest[1]) + + (p[2]-closest[2])*(p[2]-closest[2]); + } + + // region BC (edge v1-v2) + double va = d3*d6 - d5*d4; + if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) { + double w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + out_u = 1 - w; out_v = w; + double bc[3] = {v2[0] - v1[0], v2[1] - v1[1], v2[2] - v1[2]}; + double closest[3] = {v1[0] + w*bc[0], v1[1] + w*bc[1], v1[2] + w*bc[2]}; + return (p[0]-closest[0])*(p[0]-closest[0]) + + (p[1]-closest[1])*(p[1]-closest[1]) + + (p[2]-closest[2])*(p[2]-closest[2]); + } + + // region ABC (inside triangle) + double denom = 1.0 / (va + vb + vc); + double u = vb * denom; + double v = vc * denom; + out_u = u; out_v = v; + double closest[3] = {v0[0] + u*ab[0] + v*ac[0], + v0[1] + u*ab[1] + v*ac[1], + v0[2] + u*ab[2] + v*ac[2]}; + return (p[0]-closest[0])*(p[0]-closest[0]) + + (p[1]-closest[1])*(p[1]-closest[1]) + + (p[2]-closest[2])*(p[2]-closest[2]); +} + + +// query BVH for closest face to point p, return distance, face index and barycentric coordinates +void queryClosestBVHWithFace(const mjtNum* bvh, const int* child, const int* nodeid, + const double* vert, const int* face, int node_idx, + const double* p, double& best_dist_sq, + int& best_face, double& best_u, double& best_v) { + const mjtNum* aabb = &bvh[node_idx * 6]; + if (pointBoxDistSq(p, aabb) >= best_dist_sq) return; + + int left = child[node_idx * 2]; + int right = child[node_idx * 2 + 1]; + + if (left == -1 && right == -1) { + int fi = nodeid[node_idx]; + if (fi >= 0) { + const double* v0 = vert + face[fi * 3 + 0] * 3; + const double* v1 = vert + face[fi * 3 + 1] * 3; + const double* v2 = vert + face[fi * 3 + 2] * 3; + double u, v; + double dist_sq = pointTriDistSqWithUV(p, v0, v1, v2, u, v); + if (dist_sq < best_dist_sq) { + best_dist_sq = dist_sq; + best_face = fi; + best_u = u; + best_v = v; + } + } + return; + } + + if (left >= 0) { + queryClosestBVHWithFace(bvh, child, nodeid, vert, face, left, p, + best_dist_sq, best_face, best_u, best_v); + } + if (right >= 0) { + queryClosestBVHWithFace(bvh, child, nodeid, vert, face, right, p, + best_dist_sq, best_face, best_u, best_v); + } +} + + +double querySignedDistance(const mjtNum* bvh, const int* child, const int* nodeid, + int nbvh, const double* point, + const double* vert, const int* face) { + if (nbvh == 0) { + return 0; + } + + double best_dist_sq = 1e20; + int best_face = -1; + double best_u = 0, best_v = 0; + queryClosestBVHWithFace(bvh, child, nodeid, vert, face, 0, point, + best_dist_sq, best_face, best_u, best_v); + double dist = std::sqrt(best_dist_sq); + + double sign = 1.0; + if (best_face >= 0) { + const double* v0 = vert + face[best_face * 3 + 0] * 3; + const double* v1 = vert + face[best_face * 3 + 1] * 3; + const double* v2 = vert + face[best_face * 3 + 2] * 3; + + double e1[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]}; + double e2[3] = {v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]}; + double normal[3] = { + e1[1]*e2[2] - e1[2]*e2[1], + e1[2]*e2[0] - e1[0]*e2[2], + e1[0]*e2[1] - e1[1]*e2[0] + }; + + double closest[3] = { + v0[0] + best_u*(v1[0]-v0[0]) + best_v*(v2[0]-v0[0]), + v0[1] + best_u*(v1[1]-v0[1]) + best_v*(v2[1]-v0[1]), + v0[2] + best_u*(v1[2]-v0[2]) + best_v*(v2[2]-v0[2]) + }; + + double u[3] = {point[0]-closest[0], point[1]-closest[1], point[2]-closest[2]}; + double dot = u[0]*normal[0] + u[1]*normal[1] + u[2]*normal[2]; + double normal_len = mjuu_normvec(normal, 3); + double eps = 1e-12 * normal_len * dist; + sign = (dot > eps) ? 1.0 : -1.0; + } + + return sign * dist; +} + +} // namespace + + +double mjCBoundingVolumeHierarchy::QuerySignedDistance( + const double* point, const double* vert, const int* face) const { + return querySignedDistance(bvh_.data(), child_.data(), nodeid_.data(), + nbvh_, point, vert, face); +} + + +void mjCOctree::ComputeSdfCoeffs(const double* vert, int nvert, const int* face, int nface, + const mjCBoundingVolumeHierarchy& tree) { + std::vector coeffs(nvert_, 0.0); + std::vector processed(nvert_, false); std::deque queue; if (NumNodes() > 0) { @@ -647,8 +846,16 @@ void mjCOctree::ComputeSdfCoeffs(const double* vert, int nvert, continue; } if (Hang(vert_id).empty()) { - coeffs[vert_id] = sdf.signed_distance(Vert(vert_id)).distance; + // transform from octree frame (body inertial) back to mesh frame + double p_mesh[3]; + mjuu_rotVecQuat(p_mesh, Vert(vert_id), iquat_); + p_mesh[0] += ipos_[0]; + p_mesh[1] += ipos_[1]; + p_mesh[2] += ipos_[2]; + + coeffs[vert_id] = tree.QuerySignedDistance(p_mesh, vert, face); } else { + // hanging node: interpolate from parents double sum_coeff = 0; for (int dep_id : Hang(vert_id)) { sum_coeff += coeffs[dep_id]; @@ -666,6 +873,42 @@ void mjCOctree::ComputeSdfCoeffs(const double* vert, int nvert, } } + // optional Laplacian smoothing (smooths octree level transitions) + if (smoothing_iterations_ > 0) { + // build vertex neighbor graph from octree connectivity + std::vector> neighbors(nvert_); + for (int i = 0; i < NumNodes(); ++i) { + static const int edges[12][2] = { + {0, 1}, {2, 3}, {4, 5}, {6, 7}, + {0, 2}, {1, 3}, {4, 6}, {5, 7}, + {0, 4}, {1, 5}, {2, 6}, {3, 7} + }; + for (const auto& edge : edges) { + int v0 = VertId(i, edge[0]); + int v1 = VertId(i, edge[1]); + neighbors[v0].insert(v1); + neighbors[v1].insert(v0); + } + } + + // apply Laplacian smoothing + const double alpha = 0.2; + std::vector sdf_new(nvert_); + for (int iter = 0; iter < smoothing_iterations_; ++iter) { + for (int i = 0; i < nvert_; ++i) { + if (neighbors[i].empty()) { + sdf_new[i] = coeffs[i]; + } else { + double avg = 0; + for (int j : neighbors[i]) avg += coeffs[j]; + avg /= neighbors[i].size(); + sdf_new[i] = (1 - alpha) * coeffs[i] + alpha * avg; + } + } + std::swap(coeffs, sdf_new); + } + } + // copy coefficients to the octree nodes for (int i = 0; i < NumNodes(); ++i) { for (int j = 0; j < 8; j++) { @@ -680,7 +923,7 @@ static double dot2(const double* a, const double* b) { } -// From M. Schwarz and H.-P. Seidel, "Fast Parallel Surface and Solid Voxelization on GPUs". +// from M. Schwarz and H.-P. Seidel, "Fast Parallel Surface and Solid Voxelization on GPUs". static bool boxTriangle(const Triangle& v, const double aamm[6]) { // bounding box tests for (int i = 0; i < 3; i++) { diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 2e5ad168..8399a146 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -200,6 +200,10 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ { + sizeof(int) * nodeid_.size() + sizeof(int) * level_.size(); } + // query signed distance from point to mesh surface + double QuerySignedDistance(const double* point, const double* vert, + const int* face) const; + private: // internal class used during BVH construction, for partial sorting of bounding volumes struct BVElement { @@ -275,6 +279,7 @@ struct mjCOctree_ { std::vector> hang_; // hanging nodes status (nvert x 1) double ipos_[3] = {0, 0, 0}; double iquat_[4] = {1, 0, 0, 0}; + int smoothing_iterations_ = 0; // Laplacian smoothing iterations (0 = disabled) }; class mjCOctree : public mjCOctree_ { @@ -303,8 +308,13 @@ class mjCOctree : public mjCOctree_ { void AddCoeff(int n, int v, double coeff) { node_[n].coeff[v] = coeff; } double Coeff(int n, int v) const { return node_[n].coeff[v]; } - // compute SDF coefficients at octree vertices using triangle mesh distance - void ComputeSdfCoeffs(const double* vert, int nvert, const int* face, int nface); + // Set number of Laplacian smoothing iterations (0 = disabled, default) + void SetSmoothingIterations(int iterations) { smoothing_iterations_ = iterations; } + int SmoothingIterations() const { return smoothing_iterations_; } + + // compute SDF coefficients via BVH queries, optionally with Laplacian smoothing + void ComputeSdfCoeffs(const double* vert, int nvert, const int* face, int nface, + const mjCBoundingVolumeHierarchy& tree); private: void Make(std::vector& elements); diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 41528d23..a86fc6b6 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -2682,5 +2682,150 @@ TEST_F(UserObjectsTest, ZeroMass) { mj_deleteModel(model); } + +// ------------- test Octree SDF computation ----------------------------------- + +using OctreeSDFTest = MujocoTest; + +TEST_F(OctreeSDFTest, SphereSDF) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + mjData* data = mj_makeData(model); + ASSERT_THAT(data, NotNull()); + + EXPECT_GT(model->nmesh, 0); + EXPECT_EQ(model->geom_type[0], mjGEOM_SDF); + + int geom_id = 0; + int mesh_id = model->geom_dataid[geom_id]; + mjSDF sdf; + const mjpPlugin* null_plugin = nullptr; + sdf.plugin = &null_plugin; + sdf.id = &mesh_id; + sdf.type = mjSDFTYPE_SINGLE; + sdf.geomtype = (mjtGeom*)(model->geom_type + geom_id); + + // Analytic SDF for unit sphere: distance = |p| - 1 + auto analyticSdf = [](const mjtNum* p) -> double { + return mju_sqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2]) - 1.0; + }; + + int sign_errors = 0; + int total_points = 0; + double sum_sq_error = 0.0; + + // Test grid of points + for (double x = -2.0; x <= 2.0; x += 0.5) { + for (double y = -2.0; y <= 2.0; y += 0.5) { + for (double z = -2.0; z <= 2.0; z += 0.5) { + mjtNum p[3] = {x, y, z}; + double sdf_dist = mjc_distance(model, data, &sdf, p); + double gt_dist = analyticSdf(p); + + if ((sdf_dist < 0) != (gt_dist < 0)) { + sign_errors++; + } + + double error = sdf_dist - gt_dist; + sum_sq_error += error * error; + total_points++; + } + } + } + + double rmse = mju_sqrt(sum_sq_error / total_points); + + EXPECT_LT(sign_errors, total_points / 200) + << "No more than 0.5% of points should have sign errors"; + EXPECT_LT(rmse, 0.11) << "RMSE should be less than 0.11"; + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(OctreeSDFTest, TorusSDF) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + mjData* data = mj_makeData(model); + ASSERT_THAT(data, NotNull()); + + EXPECT_GT(model->nmesh, 0); + EXPECT_EQ(model->geom_type[0], mjGEOM_SDF); + + int geom_id = 0; + int mesh_id = model->geom_dataid[geom_id]; + mjSDF sdf; + const mjpPlugin* null_plugin = nullptr; + sdf.plugin = &null_plugin; + sdf.id = &mesh_id; + sdf.type = mjSDFTYPE_SINGLE; + sdf.geomtype = (mjtGeom*)(model->geom_type + geom_id); + + // Analytic SDF for torus: distance = |p_proj| - r, where p_proj is + // projection of p onto circle of radius R, and r is minor radius. + // R=1, r=0.3 + auto analyticSdf = [](const mjtNum* p) -> double { + double xy = mju_sqrt(p[0]*p[0] + p[1]*p[1]); + double vec[2] = {xy - 1.0, p[2]}; + return mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1]) - 0.3; + }; + + int sign_errors = 0; + int total_points = 0; + double sum_sq_error = 0.0; + + // Test grid of points + for (double x = -2.0; x <= 2.0; x += 0.5) { + for (double y = -2.0; y <= 2.0; y += 0.5) { + for (double z = -2.0; z <= 2.0; z += 0.5) { + mjtNum p[3] = {x, y, z}; + double sdf_dist = mjc_distance(model, data, &sdf, p); + double gt_dist = analyticSdf(p); + + if ((sdf_dist < 0) != (gt_dist < 0)) { + sign_errors++; + } + + double error = sdf_dist - gt_dist; + sum_sq_error += error * error; + total_points++; + } + } + } + + double rmse = mju_sqrt(sum_sq_error / total_points); + + EXPECT_LT(sign_errors, total_points / 20) + << "No more than 5% of points should have sign errors"; + EXPECT_LT(rmse, 0.52) << "RMSE should be close to 0.516"; + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco