Use the geom frame for SDF collisions.

Also disables mesh reorientation for meshes autogenerated with marching cubes from analytic SDFs.

PiperOrigin-RevId: 781487195
Change-Id: I36b66384f240a764823c489f78f22ba9fe32f15f

Author: quagla

src/engine/engine_collision_sdf.c

@@ -480,24 +480,6 @@ static void mapPose(const mjtNum xpos1[3], const mjtNum xquat1[4],
   mju_quat2Mat(mat12, quat12);
 }
 
-// subtract mesh position from sdf transformation
-static void undoTransformation(const mjModel* m, const mjData* d, int g,
-                               mjtNum sdf_xpos[3], mjtNum sdf_quat[4]) {
-  mjtNum* xpos = d->geom_xpos + 3 * g;
-  mjtNum* xmat = d->geom_xmat + 9 * g;
-  if (m->geom_type[g] == mjGEOM_MESH || m->geom_type[g] == mjGEOM_SDF) {
-    mjtNum negpos[3], negquat[4], xquat[4];
-    mjtNum* pos = m->mesh_pos + 3 * m->geom_dataid[g];
-    mjtNum* quat = m->mesh_quat + 4 * m->geom_dataid[g];
-    mju_mat2Quat(xquat, xmat);
-    mju_negPose(negpos, negquat, pos, quat);
-    mju_mulPose(sdf_xpos, sdf_quat, xpos, xquat, negpos, negquat);
-  } else {
-    mju_copy3(sdf_xpos, xpos);
-    mju_mat2Quat(sdf_quat, xmat);
-  }
-}
-
 //---------------------------- narrow phase -----------------------------------------------
 
 // comparison function for contact sorting
@@ -767,8 +749,7 @@ int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int
 
 // collision between a mesh and a signed distance field
 int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
-  mjtNum* pos1 = d->geom_xpos + 3 * g1;
-  mjtNum* mat1 = d->geom_xmat + 9 * g1;
+  mjGETINFO;
 
   mjtNum offset[3], rotation[9], corners[9], x[3], depth;
   mjtNum points[3*MAXSDFFACE], dist[MAXMESHPNT], candidate[3*MAXMESHPNT];
@@ -790,10 +771,10 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
   sdf.geomtype = &geomtype;
 
   // compute transformation from g1 to g2
-  mjtNum pos2true[3], sdf_quat[4], quat1[4];
+  mjtNum sdf_quat[4], quat1[4];
   mju_mat2Quat(quat1, mat1);
-  undoTransformation(m, d, g2, pos2true, sdf_quat);
-  mapPose(pos1, quat1, pos2true, sdf_quat, offset, rotation);
+  mju_mat2Quat(sdf_quat, mat2);
+  mapPose(pos1, quat1, pos2, sdf_quat, offset, rotation);
 
   // binary tree search
   collideBVH(m, (mjData*)d, g1, offset, rotation, faces, &npoints, &n0, &sdf);
@@ -845,7 +826,7 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
 
   // add only the first mjMAXCONPAIR pairs
   for (int i=0; i < mju_min(ncandidate, mjMAXCONPAIR); i++) {
-    cnt = addContact(points, con, candidate + 3*index[i], pos2true, sdf_quat,
+    cnt = addContact(points, con, candidate + 3*index[i], pos2, sdf_quat,
                      dist[index[i]], cnt, m, &sdf, (mjData*)d);
   }
 
@@ -871,17 +852,13 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
 
   // compute transformations from/to g1 to/from g2
   mjtNum quat1[4], quat2[4];
-  mjtNum pos1true[3], offset21[3], rotation21[9], rotation12[9];
-  mjtNum pos2true[3], offset1[3], rotation1[9], offset12[3];
-  mjtNum offset2[3], rotation2[9], squat1[4], squat2[4];
-  undoTransformation(m, d, g1, pos1true, squat1);
-  undoTransformation(m, d, g2, pos2true, squat2);
+  mjtNum offset21[3], rotation21[9], rotation12[9];
+  mjtNum offset12[3], offset2[3], rotation2[9];
   mju_mat2Quat(quat1, mat1);
   mju_mat2Quat(quat2, mat2);
-  mapPose(pos2, quat2, pos1, quat1, offset1, rotation1);
-  mapPose(pos1, quat1, pos1true, squat1, offset2, rotation2);
-  mapPose(pos2true, squat2, pos1true, squat1, offset21, rotation21);
-  mapPose(pos1true, squat1, pos2true, squat2, offset12, rotation12);
+  mapPose(pos1, quat1, pos1, quat1, offset2, rotation2);
+  mapPose(pos2, quat2, pos1, quat1, offset21, rotation21);
+  mapPose(pos1, quat1, pos2, quat2, offset12, rotation12);
 
   // axis-aligned bounding boxes in g1 frame
   for (int i=0; i < 8; i++) {
@@ -893,8 +870,8 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
     vec2[1] = (i&2 ? size2[1]+size2[4] : size2[1]-size2[4]);
     vec2[2] = (i&4 ? size2[2]+size2[5] : size2[2]-size2[5]);
 
-    mju_mulMatVec3(vec2, rotation1, vec2);
-    mju_addTo3(vec2, offset1);
+    mju_mulMatVec3(vec2, rotation21, vec2);
+    mju_addTo3(vec2, offset21);
 
     for (int k=0; k < 3; k++) {
       aabb1[0+k] = mju_min(aabb1[0+k], vec1[k]);
@@ -983,7 +960,7 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
 
     // contact point and normal - we use the midsurface where SDF1=SDF2 as zero level set
     sdf.type = mjSDFTYPE_MIDSURFACE;
-    cnt = addContact(contacts, con, x, pos2true, squat2, dist, cnt, m, &sdf, (mjData*)d);
+    cnt = addContact(contacts, con, x, pos2, quat2, dist, cnt, m, &sdf, (mjData*)d);
 
     // SHOULD NOT OCCUR
     if (cnt > mjMAXCONPAIR) {

src/engine/engine_ray.c

@@ -22,6 +22,7 @@
 #include <mujoco/mjmodel.h>
 #include <mujoco/mjsan.h>  // IWYU pragma: keep
 #include <mujoco/mjvisualize.h>
+#include "engine/engine_collision_sdf.h"
 #include "engine/engine_io.h"
 #include "engine/engine_plugin.h"
 #include "engine/engine_util_blas.h"
@@ -743,41 +744,35 @@ mjtNum ray_sdf(const mjModel* m, const mjData* d, int g,
     return -1;
   }
 
-  // get sdf
+  // get sdf plugin
   int instance = m->geom_plugin[g];
-  const int nslot = mjp_pluginCount();
-  const int slot = m->plugin[instance];
-  const mjpPlugin* sdf = mjp_getPluginAtSlotUnsafe(slot, nslot);
-  if (!sdf) mjERROR("invalid plugin slot: %d", slot);
-  if (!(sdf->capabilityflags & mjPLUGIN_SDF)) {
-    mjERROR("Plugin is not a sign distance field at slot %d", slot);
-  }
+  const mjpPlugin* sdf_ptr = instance == -1 ? NULL : mjc_getSDF(m, g);
+  instance = instance == -1 ? m->geom_dataid[g] : instance;
+  mjtGeom geomtype = mjGEOM_SDF;
+
+  // construct sdf struct
+  mjSDF sdf;
+  sdf.id = &instance;
+  sdf.type = mjSDFTYPE_SINGLE;
+  sdf.plugin = &sdf_ptr;
+  sdf.geomtype = &geomtype;
 
   // reset counter
-  sdf->reset(m, NULL, (void*)(d->plugin_data[instance]), instance);
-
-  // compute transformation
-  mjtNum sdf_quat[4], sdf_xmat[9], sdf_xpos[9];
-  mjtNum negpos[3], negquat[4], xquat[4];
-  mjtNum* xpos = d->geom_xpos + 3*g;
-  mjtNum* pos = m->mesh_pos + 3*m->geom_dataid[g];
-  mjtNum* quat = m->mesh_quat + 4*m->geom_dataid[g];
-  mju_mat2Quat(xquat, d->geom_xmat + 9*g);
-  mju_negPose(negpos, negquat, pos, quat);
-  mju_mulPose(sdf_xpos, sdf_quat, xpos, xquat, negpos, negquat);
-  mju_quat2Mat(sdf_xmat, sdf_quat);
+  if (sdf_ptr) {
+    sdf_ptr->reset(m, NULL, (void*)(d->plugin_data[instance]), instance);
+  }
 
   // map to local frame
   mjtNum lpnt[3], lvec[3];
-  ray_map(sdf_xpos, sdf_xmat, pnt, vec, lpnt, lvec);
+  ray_map(d->geom_xpos + 3*g, d->geom_xmat + 9*g, pnt, vec, lpnt, lvec);
 
   // unit direction
   mju_normalize3(lvec);
 
   // ray marching, see e.g. https://en.wikipedia.org/wiki/Ray_marching
   for (int i=0; i < 40; i++) {
     mju_addScl3(p, lpnt, lvec, distance_total);
-    mjtNum distance = sdf->sdf_distance(p, (mjData*)d, instance);
+    mjtNum distance = mjc_distance(m, d, &sdf, p);
     distance_total += distance;
     if (mju_abs(distance) < kMinDist) {
       return distance_total;
@@ -789,7 +784,9 @@ mjtNum ray_sdf(const mjModel* m, const mjData* d, int g,
   }
 
   // reset counter
-  sdf->reset(m, NULL, (void*)(d->plugin_data[instance]), instance);
+  if (sdf_ptr) {
+    sdf_ptr->reset(m, NULL, (void*)(d->plugin_data[instance]), instance);
+  }
 
   return -1;
 }

src/user/user_mesh.cc

@@ -141,6 +141,7 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
   processed_ = false;
   visual_ = true;
   needoct_ = false;
+  needreorient_ = true;
 
   // reset to default if given
   if (_def) {
@@ -360,6 +361,7 @@ void mjCMesh::LoadSDF() {
     userface.push_back(index);
   }
 
+  needreorient_ = false;
   normal_ = std::move(usernormal);
   face_ = std::move(userface);
   ProcessVertices(uservert);
@@ -1533,22 +1535,26 @@ void mjCMesh::Process() {
   boxsz_[1] = 0.5 * std::sqrt(6*(eigval[0] + eigval[2] - eigval[1])/volume);
   boxsz_[2] = 0.5 * std::sqrt(6*(eigval[0] + eigval[1] - eigval[2])/volume);
 
-  // make octree in the geom frame
-  // TODO: make octree in the mesh frame, update engine_collision_sdf
+  // prevent reorientation if the mesh was autogenerated using marching cubes
+  if (!needreorient_) {
+    mjuu_setvec(CoM, 0, 0, 0);
+    mjuu_setvec(quattmp, 1, 0, 0, 0);
+  }
+
+  // transform CoM to origin
+  for (int i=0; i < nvert(); i++) {
+    vert_[3*i + 0] -= CoM[0];
+    vert_[3*i + 1] -= CoM[1];
+    vert_[3*i + 2] -= CoM[2];
+  }
+  Rotate(quattmp);
+
+  // make octree in mesh frame
   if (!needoct_) {
     octree_.Clear();
   } else if (octree_.Nodes().empty()) {
-    double aamm[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
-    for (int i = 0; i < nvert(); i++) {
-      aamm[0] = std::min(aamm[0], vert_[3*i + 0]);
-      aamm[3] = std::max(aamm[3], vert_[3*i + 0]);
-      aamm[1] = std::min(aamm[1], vert_[3*i + 1]);
-      aamm[4] = std::max(aamm[4], vert_[3*i + 1]);
-      aamm[2] = std::min(aamm[2], vert_[3*i + 2]);
-      aamm[5] = std::max(aamm[5], vert_[3*i + 2]);
-    }
     octree_.SetFace(vert_, face_);
-    octree_.CreateOctree(aamm);
+    octree_.CreateOctree(aamm_);
 
     // compute sdf coefficients
     if (!plugin.active) {
@@ -1569,14 +1575,6 @@ void mjCMesh::Process() {
     }
   }
 
-  // transform CoM to origin
-  for (int i=0; i < nvert(); i++) {
-    vert_[3*i + 0] -= CoM[0];
-    vert_[3*i + 1] -= CoM[1];
-    vert_[3*i + 2] -= CoM[2];
-  }
-  Rotate(quattmp);
-
   // save the pos and quat that was used to transform the mesh
   mjuu_copyvec(pos_, CoM, 3);
   mjuu_copyvec(quat_, quattmp, 4);

src/user/user_objects.h

@@ -1003,6 +1003,7 @@ class mjCMesh_ : public mjCBase {
   std::vector<int> spec_facetexcoord_;
 
   // used by the compiler
+  bool needreorient_;                            // needs reorientation
   bool needoct_;                                 // needs octree
   bool visual_;                                  // true: the mesh is only visual
   std::vector< std::pair<int, int> > halfedge_;  // half-edge data