Refactor SDF coefficient computation into mjCOctree.

The logic for computing Signed Distance Function (SDF) coefficients using TriangleMeshDistance is moved from `mjCPlugin::Make` in `user_mesh.cc` to a new method `mjCOctree::ComputeSdfCoeffs` in `user_objects.cc`. This encapsulates the SDF computation within the octree class.

PiperOrigin-RevId: 874572414
Change-Id: Ia2365c9c4527a252ff1d1bb43fe2c2593761e471

src/user/user_mesh.cc

@@ -34,7 +34,6 @@
 
 #include <mujoco/mjspec.h>
 #include "user/user_api.h"
-#include <TriangleMeshDistance/include/tmd/TriangleMeshDistance.h>
 
 #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<double> coeffs(octree_.NumVerts());
-      std::vector<bool> processed(octree_.NumVerts(), false);
-      std::deque<int> 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());
     }
   }
 

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 <TriangleMeshDistance/include/tmd/TriangleMeshDistance.h>
 
 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<size_t>(nvert),
+                                face, static_cast<size_t>(nface));
+
+  std::vector<double> coeffs(NumVerts());
+  std::vector<bool> processed(NumVerts(), false);
+  std::deque<int> 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];
 }

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<Triangle>& elements);