diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 5d060117..70d78bed 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -34,7 +34,6 @@ #include #include "user/user_api.h" -#include #ifdef MUJOCO_TINYOBJLOADER_IMPL #define TINYOBJLOADER_IMPLEMENTATION @@ -780,52 +779,7 @@ void mjCMesh::TryCompile(const mjVFS* vfs) { // compute sdf coefficients if (!plugin.active) { - tmd::TriangleMeshDistance sdf(vert_.data(), nvert(), face_.data(), nface()); - - std::vector coeffs(octree_.NumVerts()); - std::vector processed(octree_.NumVerts(), false); - std::deque queue; - - if (octree_.NumNodes() > 0) { - queue.push_back(0); // start traversal from the root node - } - - while (!queue.empty()) { - int node_idx = queue.front(); - queue.pop_front(); - - for (int j = 0; j < 8; ++j) { - int vert_id = octree_.VertId(node_idx, j); - if (processed[vert_id]) { - continue; - } - if (octree_.Hang(vert_id).empty()) { - coeffs[vert_id] = sdf.signed_distance(octree_.Vert(vert_id)).distance; - } else { - double sum_coeff = 0; - for (int dep_id : octree_.Hang(vert_id)) { - sum_coeff += coeffs[dep_id]; - if (!processed[dep_id]) { - throw mjCError(this, "sdf coefficient computation failed"); - } - } - coeffs[vert_id] = sum_coeff / octree_.Hang(vert_id).size(); - } - processed[vert_id] = true; - } - - for (int child_idx : octree_.Children(node_idx)) { - if (child_idx != -1) { - queue.push_back(child_idx); - } - } - } - - for (int i = 0; i < octree_.NumNodes(); ++i) { - for (int j = 0; j < 8; j++) { - octree_.AddCoeff(i, j, coeffs[octree_.VertId(i, j)]); - } - } + octree_.ComputeSdfCoeffs(vert_.data(), nvert(), face_.data(), nface()); } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 4b5ee2a4..06045081 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -47,6 +47,7 @@ #include "user/user_model.h" #include "user/user_resource.h" #include "user/user_util.h" +#include namespace { namespace mju = ::mujoco::util; @@ -621,6 +622,59 @@ 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)); + + std::vector coeffs(NumVerts()); + std::vector processed(NumVerts(), false); + std::deque queue; + + if (NumNodes() > 0) { + queue.push_back(0); + } + + while (!queue.empty()) { + int node_idx = queue.front(); + queue.pop_front(); + + // compute SDF coefficients at the 8 vertices of the octree node + for (int j = 0; j < 8; ++j) { + int vert_id = VertId(node_idx, j); + if (processed[vert_id]) { + continue; + } + if (Hang(vert_id).empty()) { + coeffs[vert_id] = sdf.signed_distance(Vert(vert_id)).distance; + } else { + double sum_coeff = 0; + for (int dep_id : Hang(vert_id)) { + sum_coeff += coeffs[dep_id]; + } + coeffs[vert_id] = sum_coeff / Hang(vert_id).size(); + } + processed[vert_id] = true; + } + + // add children to the queue + for (int child_idx : Children(node_idx)) { + if (child_idx != -1) { + queue.push_back(child_idx); + } + } + } + + // copy coefficients to the octree nodes + for (int i = 0; i < NumNodes(); ++i) { + for (int j = 0; j < 8; j++) { + AddCoeff(i, j, coeffs[VertId(i, j)]); + } + } +} + + static double dot2(const double* a, const double* b) { return a[0] * b[0] + a[1] * b[1]; } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 081791d7..85a1d5e1 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -301,6 +301,10 @@ class mjCOctree : public mjCOctree_ { face_.clear(); } 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); private: void Make(std::vector& elements);