diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index c30ba7ac..46ba842a 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -780,11 +780,15 @@ void mjCMesh::TryCompile(const mjVFS* vfs) { if (!plugin.active) { tmd::TriangleMeshDistance sdf(vert_.data(), nvert(), face_.data(), nface()); + std::vector coeffs(octree_.NumVerts()); + for (int i = 0; i < octree_.NumVerts(); ++i) { + coeffs[i] = sdf.signed_distance(octree_.Vert(i)).distance; + } + // TODO: the value at hanging vertices should be computed from the parent for (int i = 0; i < octree_.NumNodes(); ++i) { for (int j = 0; j < 8; j++) { - const double* v = octree_.Vert(i, j); - octree_.AddCoeff(i, j, sdf.signed_distance(v).distance); + octree_.AddCoeff(i, j, coeffs[octree_.VertId(i, j)]); } } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 86bfd61e..3d5b983b 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -275,7 +275,8 @@ class mjCOctree : public mjCOctree_ { void CopyChild(int* child) const; void CopyAabb(mjtNum* aabb) const; void CopyCoeff(mjtNum* coeff) const; - const double* Vert(int n, int v) const { return vert_[node_[n].vertid[v]].p.data(); } + const double* Vert(int i) const { return vert_[i].p.data(); } + int VertId(int n, int v) const { return node_[n].vertid[v]; } void SetFace(const std::vector& vert, const std::vector& face); int Size() const { return sizeof(OctNode) * node_.size() + sizeof(Triangle) * face_.size() +