Remove sdflib plugin and move interpolation to engine_collision_sdf.

Before:
```
 Simulation time      : 1.71 s
 Steps per second     : 5854
 Realtime factor      : 11.71 x
 Time per step        : 170.8 µs

 Newton iters / step  : 2.19
 Contacts / step      : 3.45
 Constraints / step   : 13.79
 Degrees of freedom   : 12
 Dynamic memory usage : 0.2% of 14M
```

After:
```
 Simulation time      : 1.41 s
 Steps per second     : 7093
 Realtime factor      : 14.19 x
 Time per step        : 141.0 µs

 Newton iters / step  : 2.19
 Contacts / step      : 3.45
 Constraints / step   : 13.79
 Degrees of freedom   : 12
 Dynamic memory usage : 0.2% of 14M
```

PiperOrigin-RevId: 781087896
Change-Id: Iaf69dccc5e95af5bac862344683c74f962162186

model/plugin/sdf/cow.xml

@@ -8,19 +8,12 @@
         <config key="radius2" value="0.05"/>
       </instance>
     </plugin>
-    <plugin plugin="mujoco.sdf.sdflib">
-      <instance name="sdf">
-        <config key="aabb" value="0"/>
-      </instance>
-    </plugin>
   </extension>
 
   <asset>
     <texture name="texspot" type="2d" file="spot.png"/>
     <material name="matspot" texture="texspot"/>
-    <mesh name="spot" file="asset/spot.obj">
+    <mesh name="spot" file="asset/spot.obj"/>
-      <plugin instance="sdf"/>
-    </mesh>
     <mesh name="torus">
       <plugin instance="torus"/>
     </mesh>
@@ -48,15 +41,11 @@
       </geom>
     </body>
     <body euler="90 0 0" pos="0 0 .7">
-      <geom type="sdf" name="cow1" mesh="spot" material="matspot">
+      <geom type="sdf" name="cow1" mesh="spot" material="matspot"/>
-        <plugin instance="sdf"/>
-      </geom>
     </body>
     <body pos="0.05 .25 2.2">
       <freejoint/>
-      <geom type="sdf" name="cow2" mesh="spot" material="matspot">
+      <geom type="sdf" name="cow2" mesh="spot" material="matspot"/>
-        <plugin instance="sdf"/>
-      </geom>
     </body>
     <light name="left" pos="0 0 1"/>
     <light name="right" pos="1 0 1"/>

plugin/sdf/CMakeLists.txt

@@ -28,8 +28,6 @@ set(MUJOCO_SDF_SRCS
     register.cc
     nut.cc
     nut.h
-    sdflib.cc
-    sdflib.h
     torus.cc
     torus.h
 )

plugin/sdf/README.md

@@ -62,16 +62,6 @@ Parameters:
   - `radius1` [m]: major radius (default `0.35`).
   - `radius1` [m]: minor radius (default `0.15`).
 
-### SdfLib
-
-Implemented in [sdflib.cc](sdflib.cc). Example usage in [cow.xml](../../model/plugin/sdf/cow.xml).
-
-This plugin uses the library [TriangleMeshDistance](https://github.com/InteractiveComputerGraphics/TriangleMeshDistance)
-to compute a voxel-based approximation of a user-specified mesh. The mesh can be arbitrary and not necessarily convex.
-This offers an alternative to convex-decomposed meshes. The performance is likely to be slower than that of analytic
-SDFs, since a cubic approximation has to be evaluated on the convex grid. However, the SDF generation is done
-automatically, simplifying the task of creating an SDF, which can be difficult for complex shapes.
-
 ### How to make your own SDF
 
 Create your `MySDF.h` and `MySDF.cc` files in the SDF folder, where this README is located. Implement your SDF using the

plugin/sdf/register.cc

@@ -17,7 +17,6 @@
 #include "gear.h"
 #include "nut.h"
 #include "torus.h"
-#include "sdflib.h"
 
 namespace mujoco::plugin::sdf {
 
@@ -27,7 +26,6 @@ mjPLUGIN_LIB_INIT {
   Gear::RegisterPlugin();
   Nut::RegisterPlugin();
   Torus::RegisterPlugin();
-  SdfLib::RegisterPlugin();
 }
 
 }  // namespace mujoco::plugin::sdf

plugin/sdf/sdflib.cc

@@ -1,279 +0,0 @@
-// Copyright 2022 DeepMind Technologies Limited
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <cstdint>
-#include <cstring>
-#include <optional>
-#include <utility>
-#include <vector>
-
-#include <mujoco/mjplugin.h>
-#include <mujoco/mujoco.h>
-#include "sdf.h"
-#include "sdflib.h"
-
-namespace mujoco::plugin::sdf {
-namespace {
-
-mjtNum boxProjection(mjtNum point[3], const mjtNum box[6]) {
-  mjtNum r[3] = {point[0] - box[0], point[1] - box[1], point[2] - box[2]};
-  mjtNum q[3] = {mju_abs(r[0]) - box[3], mju_abs(r[1]) - box[4],
-                 mju_abs(r[2]) - box[5]};
-  mjtNum dist_sqr = 0;
-  mjtNum eps = 1e-6;
-
-  // skip the projection if inside
-  if (q[0] <= 0 && q[1] <= 0 && q[2] <= 0) {
-    return mju_max(q[0], mju_max(q[1], q[2]));
-  }
-
-  // in-place projection inside the box if outside
-  if ( q[0] >= 0 ) {
-    dist_sqr += q[0] * q[0];
-    point[0] -= r[0] > 0 ? (q[0]+eps) : -(q[0]+eps);
-  }
-  if ( q[1] >= 0 ) {
-    dist_sqr += q[1] * q[1];
-    point[1] -= r[1] > 0 ? (q[1]+eps) : -(q[1]+eps);
-  }
-  if ( q[2] >= 0 ) {
-    dist_sqr += q[2] * q[2];
-    point[2] -= r[2] > 0 ? (q[2]+eps) : -(q[2]+eps);
-  }
-
-  return mju_sqrt(dist_sqr);
-}
-
-// find the octree leaf containing the point p, return the index of the leaf and
-// populate the weights of the interpolated function (if w is not null) and of
-// its gradient (if dw is not null) using the vertices as degrees of freedom for
-// trilinear interpolation.
-static int findOct(mjtNum w[8], mjtNum dw[8][3], const mjtNum* oct_aabb,
-                   const int* oct_child, const mjtNum p[3]) {
-  std::vector<int> stack = {0};
-  mjtNum eps = 1e-8;
-
-  while (!stack.empty()) {
-    int node = stack.back();
-    stack.pop_back();
-    mjtNum vmin[3], vmax[3];
-
-    if (node == -1) {  // SHOULD NOT OCCUR
-      mju_error("Invalid node number");
-      return -1;
-    }
-
-    for (int j = 0; j < 3; j++) {
-      vmin[j] = oct_aabb[6*node+j] - oct_aabb[6*node+3+j];
-      vmax[j] = oct_aabb[6*node+j] + oct_aabb[6*node+3+j];
-    }
-
-    // check if the point is inside the aabb of the octree node
-    if (p[0] + eps < vmin[0] || p[0] - eps > vmax[0] ||
-        p[1] + eps < vmin[1] || p[1] - eps > vmax[1] ||
-        p[2] + eps < vmin[2] || p[2] - eps > vmax[2]) {
-      continue;
-    }
-
-    mjtNum coord[3] = {(p[0] - vmin[0]) / (vmax[0] - vmin[0]),
-                       (p[1] - vmin[1]) / (vmax[1] - vmin[1]),
-                       (p[2] - vmin[2]) / (vmax[2] - vmin[2])};
-
-    // check if the node is a leaf
-    if (oct_child[8*node+0] == -1 && oct_child[8*node+1] == -1 &&
-        oct_child[8*node+2] == -1 && oct_child[8*node+3] == -1 &&
-        oct_child[8*node+4] == -1 && oct_child[8*node+5] == -1 &&
-        oct_child[8*node+6] == -1 && oct_child[8*node+7] == -1) {
-      for (int j = 0; j < 8; j++) {
-        if (w) {
-          w[j] = (j & 1 ? coord[0] : 1 - coord[0]) *
-                 (j & 2 ? coord[1] : 1 - coord[1]) *
-                 (j & 4 ? coord[2] : 1 - coord[2]);
-        }
-        if (dw) {
-          dw[j][0] = (j & 1 ? 1 : -1) *
-                     (j & 2 ? coord[1] : 1 - coord[1]) *
-                     (j & 4 ? coord[2] : 1 - coord[2]);
-          dw[j][1] = (j & 1 ? coord[0] : 1 - coord[0]) *
-                     (j & 2 ? 1 : -1) *
-                     (j & 4 ? coord[2] : 1 - coord[2]);
-          dw[j][2] = (j & 1 ? coord[0] : 1 - coord[0]) *
-                     (j & 2 ? coord[1] : 1 - coord[1]) *
-                     (j & 4 ? 1 : -1);
-        }
-      }
-      return node;
-    }
-
-    // compute which of 8 children to visit next
-    int x = coord[0] < .5 ? 1 : 0;
-    int y = coord[1] < .5 ? 1 : 0;
-    int z = coord[2] < .5 ? 1 : 0;
-    stack.push_back(oct_child[8*node + 4*z + 2*y + x]);
-  }
-
-  mju_error("Node not found");  // SHOULD NOT OCCUR
-  return -1;
-}
-
-}  // namespace
-
-// factory function
-std::optional<SdfLib> SdfLib::Create(const mjModel* m, mjData* d,
-                                     int instance) {
-  int geomid = 0;
-  for (int i = 0; i < m->ngeom; ++i) {
-    if (m->geom_plugin[i] == instance) {
-      geomid = i;
-      break;
-    }
-  }
-  return SdfLib(m, m->geom_dataid[geomid]);
-}
-
-// plugin constructor
-SdfLib::SdfLib(const mjModel* m, int meshid) {
-  int octadr = m->mesh_octadr[meshid];
-  int octnum = m->mesh_octnum[meshid];
-  oct_aabb_.assign(m->oct_aabb + 6*octadr,
-                   m->oct_aabb + 6*octadr + 6*octnum);
-  oct_child_.assign(m->oct_child + 8 * octadr,
-                    m->oct_child + 8 * octadr + 8 * octnum);
-  sdf_coeff_.assign(8 * octnum, 0);
-  memcpy(sdf_coeff_.data(), m->oct_coeff + 8*octadr, 8*octnum*sizeof(mjtNum));
-  mju_copy(box_, m->oct_aabb + 6*octadr, 6);
-}
-
-// plugin computation
-void SdfLib::Compute(const mjModel* m, mjData* d, int instance) {
-  visualizer_.Next();
-}
-
-// plugin reset
-void SdfLib::Reset() {
-  visualizer_.Reset();
-}
-
-// plugin visualization
-void SdfLib::Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
-                       mjvScene* scn, int instance) {
-  visualizer_.Visualize(m, d, opt, scn, instance);
-}
-
-// sdf
-mjtNum SdfLib::Distance(const mjtNum p[3]) const {
-  mjtNum w[8];
-  mjtNum sdf = 0;
-  mjtNum point[3] = {p[0], p[1], p[2]};
-  mjtNum boxDist = boxProjection(point, box_);
-  if (boxDist > 0) {
-    return boxDist;
-  }
-  int node = findOct(w, nullptr, oct_aabb_.data(), oct_child_.data(), point);
-  for (int i = 0; i < 8; ++i) {
-    sdf += w[i] * sdf_coeff_[8*node + i];
-  }
-  return sdf;
-}
-
-// gradient of sdf
-void SdfLib::Gradient(mjtNum grad[3], const mjtNum point[3]) const {
-
-  mjtNum p[3] = {point[0], point[1], point[2]};
-
-  // analytic in the interior
-  if (boxProjection(p, box_) <= 0) {
-    mjtNum dw[8][3];
-    mju_zero3(grad);
-    int node = findOct(nullptr, dw, oct_aabb_.data(), oct_child_.data(), p);
-    for (int i = 0; i < 8; ++i) {
-      grad[0] += dw[i][0] * sdf_coeff_[8*node + i];
-      grad[1] += dw[i][1] * sdf_coeff_[8*node + i];
-      grad[2] += dw[i][2] * sdf_coeff_[8*node + i];
-    }
-    return;
-  }
-
-  // finite difference in the exterior
-  mjtNum eps = 1e-8;
-  mjtNum dist0 = Distance(point);
-  mjtNum pointX[3] = {point[0]+eps, point[1], point[2]};
-  mjtNum distX = Distance(pointX);
-  mjtNum pointY[3] = {point[0], point[1]+eps, point[2]};
-  mjtNum distY = Distance(pointY);
-  mjtNum pointZ[3] = {point[0], point[1], point[2]+eps};
-  mjtNum distZ = Distance(pointZ);
-
-  grad[0] = (distX - dist0) / eps;
-  grad[1] = (distY - dist0) / eps;
-  grad[2] = (distZ - dist0) / eps;
-}
-
-// plugin registration
-void SdfLib::RegisterPlugin() {
-  mjpPlugin plugin;
-  mjp_defaultPlugin(&plugin);
-
-  plugin.name = "mujoco.sdf.sdflib";
-  plugin.capabilityflags |= mjPLUGIN_SDF;
-
-  const char* attributes[] = {"aabb"};
-  plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
-  plugin.attributes = attributes;
-  plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
-
-  plugin.init = +[](const mjModel* m, mjData* d, int instance) {
-    auto sdf_or_null = SdfLib::Create(m, d, instance);
-    if (!sdf_or_null.has_value()) {
-      return -1;
-    }
-    d->plugin_data[instance] = reinterpret_cast<uintptr_t>(
-        new SdfLib(std::move(*sdf_or_null)));
-    return 0;
-  };
-  plugin.destroy = +[](mjData* d, int instance) {
-    delete reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
-    d->plugin_data[instance] = 0;
-  };
-  plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data,
-                     int instance) {
-    auto sdf = reinterpret_cast<SdfLib*>(plugin_data);
-    sdf->Reset();
-  };
-  plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt,
-                         mjvScene* scn, int instance) {
-    auto* sdf = reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
-    sdf->Visualize(m, d, opt, scn, instance);
-  };
-  plugin.compute =
-      +[](const mjModel* m, mjData* d, int instance, int capability_bit) {
-        auto* sdf = reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
-        sdf->Compute(m, d, instance);
-      };
-  plugin.sdf_distance =
-      +[](const mjtNum point[3], const mjData* d, int instance) {
-        auto* sdf = reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
-        return sdf->Distance(point);
-      };
-  plugin.sdf_gradient = +[](mjtNum gradient[3], const mjtNum point[3],
-                        const mjData* d, int instance) {
-    auto* sdf = reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
-    sdf->visualizer_.AddPoint(point);
-    sdf->Gradient(gradient, point);
-  };
-
-  mjp_registerPlugin(&plugin);
-}
-
-}  // namespace mujoco::plugin::sdf

plugin/sdf/sdflib.h

@@ -1,57 +0,0 @@
-// Copyright 2022 DeepMind Technologies Limited
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MUJOCO_PLUGIN_SDF_SDFLIB_H_
-#define MUJOCO_PLUGIN_SDF_SDFLIB_H_
-
-#include <optional>
-#include <vector>
-
-#include <mujoco/mjdata.h>
-#include <mujoco/mjmodel.h>
-#include <mujoco/mjtnum.h>
-#include <mujoco/mjvisualize.h>
-#include "sdf.h"
-
-namespace mujoco::plugin::sdf {
-class SdfLib {
- public:
-  // Creates a new SdfLib instance or returns null on failure.
-  static std::optional<SdfLib> Create(const mjModel* m, mjData* d,
-                                      int instance);
-  SdfLib(SdfLib&&) = default;
-  ~SdfLib() = default;
-
-  void Reset();
-  void Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
-                 mjvScene* scn, int instance);
-  void Compute(const mjModel* m, mjData* d, int instance);
-  mjtNum Distance(const mjtNum point[3]) const;
-  void Gradient(mjtNum grad[3], const mjtNum point[3]) const;
-
-  static void RegisterPlugin();
-
- private:
-  SdfLib(const mjModel* m, int meshid);
-  SdfVisualizer visualizer_;
-  std::vector<double> sdf_coeff_;
-
-  mjtNum box_[6];
-  std::vector<mjtNum> oct_aabb_;
-  std::vector<int> oct_child_;
-};
-
-}  // namespace mujoco::plugin::sdf
-
-#endif  // MUJOCO_PLUGIN_SDF_SDFLIB_H_

src/engine/engine_collision_sdf.c

@@ -34,6 +34,168 @@
 #define MAXMESHPNT 500
 
 
+//---------------------------- interpolated sdf -------------------------------------------
+
+mjtNum boxProjection(mjtNum point[3], const mjtNum box[6]) {
+  mjtNum r[3] = {point[0] - box[0], point[1] - box[1], point[2] - box[2]};
+  mjtNum q[3] = {mju_abs(r[0]) - box[3], mju_abs(r[1]) - box[4],
+                 mju_abs(r[2]) - box[5]};
+  mjtNum dist_sqr = 0;
+  mjtNum eps = 1e-6;
+
+  // skip the projection if inside
+  if (q[0] <= 0 && q[1] <= 0 && q[2] <= 0) {
+    return mju_max(q[0], mju_max(q[1], q[2]));
+  }
+
+  // in-place projection inside the box if outside
+  if ( q[0] >= 0 ) {
+    dist_sqr += q[0] * q[0];
+    point[0] -= r[0] > 0 ? (q[0]+eps) : -(q[0]+eps);
+  }
+  if ( q[1] >= 0 ) {
+    dist_sqr += q[1] * q[1];
+    point[1] -= r[1] > 0 ? (q[1]+eps) : -(q[1]+eps);
+  }
+  if ( q[2] >= 0 ) {
+    dist_sqr += q[2] * q[2];
+    point[2] -= r[2] > 0 ? (q[2]+eps) : -(q[2]+eps);
+  }
+
+  return mju_sqrt(dist_sqr);
+}
+
+// find the octree leaf containing the point p, return the index of the leaf and
+// populate the weights of the interpolated function (if w is not null) and of
+// its gradient (if dw is not null) using the vertices as degrees of freedom for
+// trilinear interpolation.
+static int findOct(mjtNum w[8], mjtNum dw[8][3], const mjtNum* oct_aabb,
+                   const int* oct_child, const mjtNum p[3]) {
+  int stack = 0;
+  mjtNum eps = 1e-8;
+  int niter = 100;
+
+  while (niter-- > 0) {
+    int node = stack;
+    mjtNum vmin[3], vmax[3];
+
+    if (node == -1) {  // SHOULD NOT OCCUR
+      mju_error("Invalid node number");
+      return -1;
+    }
+
+    for (int j = 0; j < 3; j++) {
+      vmin[j] = oct_aabb[6*node+j] - oct_aabb[6*node+3+j];
+      vmax[j] = oct_aabb[6*node+j] + oct_aabb[6*node+3+j];
+    }
+
+    // check if the point is inside the aabb of the octree node
+    if (p[0] + eps < vmin[0] || p[0] - eps > vmax[0] ||
+        p[1] + eps < vmin[1] || p[1] - eps > vmax[1] ||
+        p[2] + eps < vmin[2] || p[2] - eps > vmax[2]) {
+      continue;
+    }
+
+    mjtNum coord[3] = {(p[0] - vmin[0]) / (vmax[0] - vmin[0]),
+                       (p[1] - vmin[1]) / (vmax[1] - vmin[1]),
+                       (p[2] - vmin[2]) / (vmax[2] - vmin[2])};
+
+    // check if the node is a leaf
+    if (oct_child[8*node+0] == -1 && oct_child[8*node+1] == -1 &&
+        oct_child[8*node+2] == -1 && oct_child[8*node+3] == -1 &&
+        oct_child[8*node+4] == -1 && oct_child[8*node+5] == -1 &&
+        oct_child[8*node+6] == -1 && oct_child[8*node+7] == -1) {
+      for (int j = 0; j < 8; j++) {
+        if (w) {
+          w[j] = (j & 1 ? coord[0] : 1 - coord[0]) *
+                 (j & 2 ? coord[1] : 1 - coord[1]) *
+                 (j & 4 ? coord[2] : 1 - coord[2]);
+        }
+        if (dw) {
+          dw[j][0] = (j & 1 ? 1 : -1) *
+                     (j & 2 ? coord[1] : 1 - coord[1]) *
+                     (j & 4 ? coord[2] : 1 - coord[2]);
+          dw[j][1] = (j & 1 ? coord[0] : 1 - coord[0]) *
+                     (j & 2 ? 1 : -1) *
+                     (j & 4 ? coord[2] : 1 - coord[2]);
+          dw[j][2] = (j & 1 ? coord[0] : 1 - coord[0]) *
+                     (j & 2 ? coord[1] : 1 - coord[1]) *
+                     (j & 4 ? 1 : -1);
+        }
+      }
+      return node;
+    }
+
+    // compute which of 8 children to visit next
+    int x = coord[0] < .5 ? 1 : 0;
+    int y = coord[1] < .5 ? 1 : 0;
+    int z = coord[2] < .5 ? 1 : 0;
+    stack = oct_child[8 * node + 4*z + 2*y + x];
+  }
+
+  mju_error("Node not found");  // SHOULD NOT OCCUR
+  return -1;
+}
+
+// sdf
+mjtNum oct_distance(const mjModel* m, const mjtNum p[3], int meshid) {
+  int octadr = m->mesh_octadr[meshid];
+  int* oct_child = m->oct_child + 8*octadr;
+  mjtNum* oct_aabb = m->oct_aabb + 6*octadr;
+  mjtNum* oct_coeff = m->oct_coeff + 8*octadr;
+
+  mjtNum w[8];
+  mjtNum sdf = 0;
+  mjtNum point[3] = {p[0], p[1], p[2]};
+  mjtNum boxDist = boxProjection(point, oct_aabb);
+  if (boxDist > 0) {
+    return boxDist;
+  }
+  int node = findOct(w, NULL, oct_aabb, oct_child, point);
+  for (int i = 0; i < 8; ++i) {
+    sdf += w[i] * oct_coeff[8*node + i];
+  }
+  return sdf;
+}
+
+// gradient of sdf
+void oct_gradient(const mjModel* m, mjtNum grad[3], const mjtNum point[3], int meshid) {
+  mju_zero3(grad);
+  mjtNum p[3] = {point[0], point[1], point[2]};
+
+  int octadr = m->mesh_octadr[meshid];
+  int* oct_child = m->oct_child + 8*octadr;
+  mjtNum* oct_aabb = m->oct_aabb + 6*octadr;
+  mjtNum* oct_coeff = m->oct_coeff + 8*octadr;
+
+  // analytic in the interior
+  if (boxProjection(p, oct_aabb) <= 0) {
+    mjtNum dw[8][3];
+    int node = findOct(NULL, dw, oct_aabb, oct_child, p);
+    for (int i = 0; i < 8; ++i) {
+      grad[0] += dw[i][0] * oct_coeff[8*node + i];
+      grad[1] += dw[i][1] * oct_coeff[8*node + i];
+      grad[2] += dw[i][2] * oct_coeff[8*node + i];
+    }
+    return;
+  }
+
+  // finite difference in the exterior
+  mjtNum eps = 1e-8;
+  mjtNum dist0 = oct_distance(m, point, meshid);
+  mjtNum pointX[3] = {point[0]+eps, point[1], point[2]};
+  mjtNum distX = oct_distance(m, pointX, meshid);
+  mjtNum pointY[3] = {point[0], point[1]+eps, point[2]};
+  mjtNum distY = oct_distance(m, pointY, meshid);
+  mjtNum pointZ[3] = {point[0], point[1], point[2]+eps};
+  mjtNum distZ = oct_distance(m, pointZ, meshid);
+
+  grad[0] = (distX - dist0) / eps;
+  grad[1] = (distY - dist0) / eps;
+  grad[2] = (distZ - dist0) / eps;
+}
+
+
 //---------------------------- primitives sdf ---------------------------------------------
 
 static void radialField3d(mjtNum field[3], const mjtNum a[3], const mjtNum x[3],
@@ -100,7 +262,11 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p
     b[1] = mju_max(a[1], 0);
     return mju_min(mju_max(a[0], a[1]), 0) + mju_norm(b, 2);
   case mjGEOM_SDF:
-    return p->sdf_distance(x, d, i);
+    if (p) {
+      return p->sdf_distance(x, d, i);
+    } else {
+      return oct_distance(m, x, i);
+    }
   default:
     mjERROR("sdf collisions not available for geom type %d", type);
     return 0;
@@ -199,7 +365,11 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d,
     }
     break;
   case mjGEOM_SDF:
-    p->sdf_gradient(gradient, x, d, i);
+    if (p) {
+      p->sdf_gradient(gradient, x, d, i);
+    } else {
+      oct_gradient(m, gradient, x, i);
+    }
     break;
   default:
     mjERROR("sdf collisions not available for geom type %d", type);
@@ -608,7 +778,8 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
 
   // get sdf plugin
   int instance = m->geom_plugin[g2];
-  const mjpPlugin* sdf_ptr = mjc_getSDF(m, g2);
+  const mjpPlugin* sdf_ptr = instance == -1 ? NULL : mjc_getSDF(m, g2);
+  instance = instance == -1 ? m->geom_dataid[g2] : instance;
   mjtGeom geomtype = mjGEOM_SDF;
 
   // copy into data
@@ -750,22 +921,26 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
   mjtGeom geomtypes[2] = {m->geom_type[g2], m->geom_type[g1]};
 
   instance[0] = m->geom_plugin[g2];
-  sdf_ptr[0] = mjc_getSDF(m, g2);
+  sdf_ptr[0] = instance[0] == -1 ? NULL : mjc_getSDF(m, g2);
 
   // get sdf plugins
   if (m->geom_type[g1] == mjGEOM_SDF) {
     instance[1] = m->geom_plugin[g1];
-    sdf_ptr[1] = mjc_getSDF(m, g1);
+    sdf_ptr[1] = instance[1] == -1 ? NULL : mjc_getSDF(m, g1);
   } else {
     instance[1] = g1;
     sdf_ptr[1] = NULL;
   }
 
   // reset visualization count
-  sdf_ptr[0]->reset(m, NULL, (void*)(d->plugin_data[instance[0]]), instance[0]);
+  if (sdf_ptr[0]) {
+    sdf_ptr[0]->reset(m, NULL, (void*)(d->plugin_data[instance[0]]), instance[0]);
+  }
 
   // copy into sdf
   mjSDF sdf;
+  instance[0] = instance[0] == -1 ? m->geom_dataid[g2] : instance[0];
+  instance[1] = instance[1] == -1 ? m->geom_dataid[g1] : instance[1];
   sdf.id = instance;
   sdf.relpos = offset21;
   sdf.relmat = rotation21;
@@ -794,7 +969,9 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
     i++;
 
     // start counters
-    sdf_ptr[0]->compute(m, (mjData*)d, instance[0], mjPLUGIN_SDF);
+    if (sdf_ptr[0]) {
+      sdf_ptr[0]->compute(m, (mjData*)d, instance[0], mjPLUGIN_SDF);
+    }
 
     // gradient descent - we use a special function of the two SDF as objective
     sdf.type = mjSDFTYPE_COLLISION;

src/user/user_mesh.cc

@@ -1551,8 +1551,7 @@ void mjCMesh::Process() {
     octree_.CreateOctree(aamm);
 
     // compute sdf coefficients
-    // TODO: only check !plugin.active once sdflib is removed
+    if (!plugin.active) {
-    if (plugin.active && *plugin.name == "sdf") {
       tmd::TriangleMeshDistance sdf(vert_.data(), nvert(), face_.data(), nface());
 
       // TODO: do not evaluate the SDF multiple times at the same vertex

test/engine/engine_plugin_test.cc

@@ -37,7 +37,7 @@ using ::testing::DoubleNear;
 using ::testing::HasSubstr;
 using ::testing::NotNull;
 
-constexpr int kNumTruePlugins = 10;
+constexpr int kNumTruePlugins = 9;
 constexpr int kNumFakePlugins = 30;
 constexpr int kNumTestPlugins = 4;