Add Signed Distance Field to collision geometries.

PiperOrigin-RevId: 557507088
Change-Id: I358a642407aee1ba8dfc9d405eb9a4f609435fe9

src/user/user_mesh.cc

@@ -27,7 +27,7 @@
 #define TINYOBJLOADER_IMPLEMENTATION
 #endif
 
-#include <mujoco/mjmacro.h>
+#include <MC.h>
 #include <mujoco/mjmodel.h>
 #include "cc/array_safety.h"
 #include "engine/engine_crossplatform.h"
@@ -256,6 +256,78 @@ void mjCMesh::set_needhull(bool needhull) {
 
 
 
+// generate mesh using marching cubes
+void mjCMesh::LoadSDF() {
+  if (plugin_name.empty() && plugin_instance_name.empty()) {
+    throw mjCError(
+        this, "neither 'plugin' nor 'instance' is specified for mesh '%s', (id = %d)",
+        name.c_str(), id);
+  }
+
+  model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
+  const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
+  if (!(plugin->capabilityflags & mjPLUGIN_SDF)) {
+    throw mjCError(this, "plugin '%s' does not support signed distance fields", plugin->name);
+  }
+
+  int i=0;
+  mjtNum attributes[10] = {0};
+  for (auto const& pair : plugin_instance->config_attribs) {
+    attributes[i++] = std::stod(pair.second);
+  }
+
+  mjtNum aabb[6] = {0};
+  plugin->sdf_aabb(aabb, attributes);
+  mjtNum total = aabb[3] + aabb[4] + aabb[5];
+
+  const mjtNum n = 300;
+  int nx, ny, nz;
+  nx = floor(n / total * aabb[3]) + 1;
+  ny = floor(n / total * aabb[4]) + 1;
+  nz = floor(n / total * aabb[5]) + 1;
+  MC::MC_FLOAT* field = new MC::MC_FLOAT[nx * ny * nz];
+
+  for (int i = 0; i < nx; i++) {
+    for (int j = 0; j < ny; j++) {
+      for (int k = 0; k < nz; k++) {
+        mjtNum point[] = {aabb[0]-aabb[3] + 2 * aabb[3] * i / (nx-1),
+                          aabb[1]-aabb[4] + 2 * aabb[4] * j / (ny-1),
+                          aabb[2]-aabb[5] + 2 * aabb[5] * k / (nz-1)};
+        field[(k * ny + j) * nx + i] =  plugin->sdf_staticdistance(point, attributes);
+      }
+    }
+  }
+
+  MC::mcMesh mesh;
+  MC::marching_cube(field, nx, ny, nz, mesh);
+  std::vector<float> uservert;
+  std::vector<float> usernormal;
+  std::vector<int> userface;
+
+  for (size_t i = 0; i < mesh.vertices.size(); i++) {
+    uservert.push_back(2*aabb[3]*mesh.vertices.at(i).x/(nx-1) + aabb[0]-aabb[3]);
+    uservert.push_back(2*aabb[4]*mesh.vertices.at(i).y/(ny-1) + aabb[1]-aabb[4]);
+    uservert.push_back(2*aabb[5]*mesh.vertices.at(i).z/(nz-1) + aabb[2]-aabb[5]);
+  }
+
+  for (size_t i = 0; i < mesh.normals.size(); i++) {
+    usernormal.push_back(mesh.normals.at(i).x);
+    usernormal.push_back(mesh.normals.at(i).y);
+    usernormal.push_back(mesh.normals.at(i).z);
+  }
+
+  for (size_t i = 0; i < mesh.indices.size(); i++) {
+    userface.push_back(mesh.indices.at(i));
+  }
+
+  set_uservert(uservert);
+  set_usernormal(usernormal);
+  set_userface(userface);
+  delete[] field;
+}
+
+
+
 // compiler
 void mjCMesh::Compile(int vfs_provider) {
   // load file
@@ -293,6 +365,11 @@ void mjCMesh::Compile(int vfs_provider) {
     }
   }
 
+  // create using marching cubes
+  if (is_plugin) {
+    LoadSDF();
+  }
+
   // copy user vertex
   if (!uservert_.empty()) {
     // check repeated

src/user/user_model.cc

@@ -2743,6 +2743,14 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, int vfs_provider) {
       }
     }
 
+    for (int i = 0; i < ngeom; ++i) {
+      if (geoms[i]->is_plugin) {
+        m->geom_plugin[i] = geoms[i]->plugin_instance->id;
+      } else {
+        m->geom_plugin[i] = -1;
+      }
+    }
+
     std::vector<std::vector<int>> plugin_to_sensors(nplugin);
     for (int i = 0; i < nsensor; ++i) {
       if (sensors[i]->type == mjSENS_PLUGIN) {

src/user/user_objects.cc

@@ -484,6 +484,12 @@ mjCBase::mjCBase() {
   xmlpos[0] = xmlpos[1] = -1;
   model = 0;
   def = 0;
+
+  // plugin variables
+  is_plugin = false;
+  plugin_instance = nullptr;
+  plugin_name = "";
+  plugin_instance_name = "";
 }
 
 
@@ -555,12 +561,6 @@ mjCBody::mjCBody(mjCModel* _model) {
   gravcomp = 0;
   userdata.clear();
 
-  // plugin variables
-  is_plugin = false;
-  plugin_instance = nullptr;
-  plugin_name = "";
-  plugin_instance_name = "";
-
   // clear object lists
   bodies.clear();
   geoms.clear();
@@ -1284,6 +1284,12 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) {
   // set model, def
   model = _model;
   def = (_def ? _def : (_model ? _model->defaults[0] : 0));
+
+  // plugin variables
+  is_plugin = false;
+  plugin_instance = nullptr;
+  plugin_name = "";
+  plugin_instance_name = "";
 }
 
 
@@ -1293,7 +1299,7 @@ double mjCGeom::GetVolume(void) {
   double height;
 
   // get from mesh
-  if (type==mjGEOM_MESH) {
+  if (type==mjGEOM_MESH || type==mjGEOM_SDF) {
     if (meshid<0 || meshid>=(int)model->meshes.size()) {
       throw mjCError(this, "invalid meshid in mesh geom '%s' (id = %d)", name.c_str(), id);
     }
@@ -1353,7 +1359,7 @@ void mjCGeom::SetInertia(void) {
   double height;
 
   // get from mesh
-  if (type==mjGEOM_MESH) {
+  if (type==mjGEOM_MESH || type==mjGEOM_SDF) {
     if (meshid<0 || meshid>=(int)model->meshes.size()) {
       throw mjCError(this, "invalid meshid in mesh geom '%s' (id = %d)", name.c_str(), id);
     }
@@ -1440,6 +1446,7 @@ double mjCGeom::GetRBound(void) {
     return sqrt(size[0]*size[0]+size[1]*size[1]+size[2]*size[2]);
 
   case mjGEOM_MESH:
+  case mjGEOM_SDF:
     aabb = model->meshes[meshid]->aabb();
     haabb[0] = mjMAX(fabs(aabb[0]), fabs(aabb[3]));
     haabb[1] = mjMAX(fabs(aabb[1]), fabs(aabb[4]));
@@ -1586,6 +1593,7 @@ void mjCGeom::ComputeAABB() {
     break;
 
   case mjGEOM_MESH:
+  case mjGEOM_SDF:
     mjuu_copyvec(aabb, model->meshes[meshid]->aabb(), 6);
     break;
 
@@ -1647,7 +1655,7 @@ void mjCGeom::Compile(void) {
   }
 
   // check mesh
-  if (type==mjGEOM_MESH && meshid<0) {
+  if ((type==mjGEOM_MESH || type==mjGEOM_SDF) && meshid<0) {
     throw mjCError(this, "mesh geom '%s' (id = %d) must have valid meshid", name.c_str(), id);
   }
 
@@ -1730,7 +1738,7 @@ void mjCGeom::Compile(void) {
 
     // fit geom if type is not mjGEOM_MESH
     double meshpos[3];
-    if (type!=mjGEOM_MESH) {
+    if (type!=mjGEOM_MESH && type!=mjGEOM_SDF) {
       pmesh->FitGeom(this, meshpos);
 
       // remove reference to mesh
@@ -1752,7 +1760,7 @@ void mjCGeom::Compile(void) {
     size[0] = model->hfields[hfieldid]->size[0];
     size[1] = model->hfields[hfieldid]->size[1];
     size[2] = 0.5*(model->hfields[hfieldid]->size[2]+model->hfields[hfieldid]->size[3]);
-  } else if (type==mjGEOM_MESH) {
+  } else if (type==mjGEOM_MESH || type==mjGEOM_SDF) {
     const double* aabb = model->meshes[meshid]->aabb();
     size[0] = mjMAX(fabs(aabb[0]), fabs(aabb[3]));
     size[1] = mjMAX(fabs(aabb[1]), fabs(aabb[4]));
@@ -1793,6 +1801,21 @@ void mjCGeom::Compile(void) {
   if (fluid_switch > 0) {
     SetFluidCoefs();
   }
+
+  // plugin
+  if (is_plugin) {
+    if (plugin_name.empty() && plugin_instance_name.empty()) {
+      throw mjCError(
+          this, "neither 'plugin' nor 'instance' is specified for geom '%s', (id = %d)",
+          name.c_str(), id);
+    }
+
+    model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
+    const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
+    if (!(plugin->capabilityflags & mjPLUGIN_SDF)) {
+      throw mjCError(this, "plugin '%s' does not support sign distance fields", plugin->name);
+    }
+  }
 }
 
 

src/user/user_objects.h

@@ -610,6 +610,7 @@ class mjCMesh: public mjCBase {
   void LoadOBJ(mjResource* resource);         // load mesh in wavefront OBJ format
   void LoadSTL(mjResource* resource);         // load mesh in STL BIN format
   void LoadMSH(mjResource* resource);         // load mesh in MSH BIN format
+  void LoadSDF();                             // generate mesh using marching cubes
   void MakeGraph(void);                       // make graph of convex hull
   void CopyGraph(void);                       // copy graph into face data
   void MakeNormal(void);                      // compute vertex normals