Refactor octree in preparation for continuous interpolation.

PiperOrigin-RevId: 792092644
Change-Id: I096ca0989210a00ae0b4556498d96212755d23c7

test/user/user_mesh_test.cc

@@ -14,6 +14,7 @@
 
 // Tests for user/user_objects.cc.
 
+#include <algorithm>
 #include <array>
 #include <cmath>
 #include <cstddef>
@@ -1287,6 +1288,53 @@ TEST_F(MjCMeshTest, OctreeNotComputedForNonSDF) {
   mj_deleteModel(model);
 }
 
+double CubeSDF(double p[3], double b[3]) {
+  double q[3] = {std::abs(p[0]) - b[0],
+                 std::abs(p[1]) - b[1],
+                 std::abs(p[2]) - b[2]};
+  return std::sqrt(std::pow(std::max(q[0], 0.0), 2) +
+                   std::pow(std::max(q[1], 0.0), 2) +
+                   std::pow(std::max(q[2], 0.0), 2)) +
+         std::min(std::max(q[0], std::max(q[1], q[2])), 0.0);
+}
+
+TEST_F(MjCMeshTest, OctreeCube) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="mesh" vertex="-1 -1 -1  1 -1 -1  -1 1 -1  1 1 -1
+                                -1 -1  1  1 -1  1  -1 1  1  1 1  1"/>
+    </asset>
+    <worldbody>
+      <geom mesh="mesh" type="sdf"/>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(m, NotNull()) << error.data();
+  EXPECT_EQ(m->noct, 54089);
+  mjData* d = mj_makeData(m);
+  ASSERT_THAT(d, NotNull());
+  mj_forward(m, d);
+  mjSDF sdf;
+  int instance = 0;
+  mjtGeom geomtype = mjGEOM_SDF;
+  const mjpPlugin* sdf_ptr = NULL;
+  sdf.id = &instance;
+  sdf.type = mjSDFTYPE_SINGLE;
+  sdf.plugin = &sdf_ptr;
+  sdf.geomtype = &geomtype;
+  mjtNum pnt[3][3] = {{1, 1, 1}, {.5, .5, .5}, {.5, 0, 0}};
+  mjtNum size[3] = {1, 1, 1};
+  for (int i = 0; i < 3; ++i) {
+    EXPECT_NEAR(mjc_distance(m, d, &sdf, pnt[i]), CubeSDF(pnt[i], size), 1e-1)
+        << "i = " << i;
+  }
+  mj_deleteModel(m);
+  mj_deleteData(d);
+}
+
 TEST_F(MjCMeshTest, HemisphereSizes) {
   static constexpr char xml[] = R"(
   <mujoco model="makemesh">