Add faster support functions for NativeCCD.

PiperOrigin-RevId: 684809392
Change-Id: I5125d4bb74c167d370aa919befba9d356c7162da

src/engine/engine_collision_convex.c

@@ -29,7 +29,6 @@
 #include "engine/engine_util_misc.h"
 #include "engine/engine_util_spatial.h"
 
-
 // call LibCCD or GJK to recover penetration info
 static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2,
                            const ccd_t* ccd, ccd_real_t* depth, ccd_vec3_t* dir, ccd_vec3_t* pos) {
@@ -73,8 +72,6 @@ void mjccd_center(const void *obj, ccd_vec3_t *center) {
   mjc_center(center->v, (const mjCCDObj*) obj);
 }
 
-
-
 // center function for convex collision algorithms
 void mjc_center(mjtNum res[3], const mjCCDObj *obj) {
   int g = obj->geom;
@@ -100,20 +97,374 @@ void mjc_center(mjtNum res[3], const mjCCDObj *obj) {
 
 
 
-// ccd support function
+// prism center function
-void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
+static void mjc_prism_center(mjtNum res[3], const mjCCDObj* obj) {
-  mjc_support(vec->v, (mjCCDObj*) obj, _dir->v);
+  // compute mean
+  mju_zero3(res);
+  for (int i=0; i < 6; i++) {
+    mju_addTo3(res, obj->prism[i]);
+  }
+  mju_scl3(res, res, 1.0/6.0);
 }
 
 
 
-// support function for convex collision algorithms
+// ccd prism center function
-void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
+static void mjccd_prism_center(const void *obj, ccd_vec3_t *center) {
+  mjc_prism_center(center->v, (const mjCCDObj*) obj);
+}
+
+// ------------------------------------ Support functions -----------------------------------------
+
+// transform a vector from global to local frame
+static inline void mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum dir[3]) {
+  // perform matT * dir
+  res[0] = mat[0]*dir[0] + mat[3]*dir[1] + mat[6]*dir[2];
+  res[1] = mat[1]*dir[0] + mat[4]*dir[1] + mat[7]*dir[2];
+  res[2] = mat[2]*dir[0] + mat[5]*dir[1] + mat[8]*dir[2];
+}
+
+
+
+// transform a vector from local to global frame
+static inline void localToGlobal(mjtNum res[3], const mjtNum mat[9], const mjtNum dir[3],
+                                 const mjtNum pos[3]) {
+  // perform mat * dir + pos
+  res[0] = mat[0]*dir[0] + mat[1]*dir[1] + mat[2]*dir[2];
+  res[1] = mat[3]*dir[0] + mat[4]*dir[1] + mat[5]*dir[2];
+  res[2] = mat[6]*dir[0] + mat[7]*dir[1] + mat[8]*dir[2];
+  res[0] += pos[0];
+  res[1] += pos[1];
+  res[2] += pos[2];
+}
+
+
+
+// sphere support function
+static void mjc_sphereSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
+  const mjModel* m = obj->model;
+  const mjData* d = obj->data;
+
+  // sphere data
+  const mjtNum* pos = d->geom_xpos + 3*obj->geom;
+  mjtNum radius = m->geom_size[3*obj->geom];
+
+  res[0] = radius*dir[0] + pos[0];
+  res[1] = radius*dir[1] + pos[1];
+  res[2] = radius*dir[2] + pos[2];
+}
+
+
+
+// capsule support function
+static void mjc_capsuleSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
+  const mjModel* m = obj->model;
+  const mjData* d = obj->data;
+
+  // capsule data
+  int i = 3*obj->geom;
+  const mjtNum* mat = d->geom_xmat + 3*i;
+  const mjtNum* pos = d->geom_xpos + i;
+  mjtNum radius = m->geom_size[i];
+  mjtNum length = m->geom_size[i+1];
+
+  // rotate dir to geom local frame
+  mjtNum local_dir[3], tmp[3];
+  mulMatTVec3(local_dir, mat, dir);
+
+  // start with sphere
+  tmp[0] = local_dir[0] * radius;
+  tmp[1] = local_dir[1] * radius;
+  tmp[2] = local_dir[2] * radius;
+
+  // add cylinder contribution
+  tmp[2] += mju_sign(local_dir[2]) * length;
+
+  // transform result to global frame
+  localToGlobal(res, mat, tmp, pos);
+}
+
+
+
+// ellipsoid support function
+static void mjc_ellipsoidSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
+  const mjModel* m = obj->model;
+  const mjData* d = obj->data;
+
+  // ellipsoid data
+  int i = 3*obj->geom;
+  const mjtNum* mat = d->geom_xmat + 3*i;
+  const mjtNum* pos = d->geom_xpos + i;
+  const mjtNum* size = m->geom_size + i;
+
+  // rotate dir to geom local frame
+  mjtNum local_dir[3], tmp[3];
+  mulMatTVec3(local_dir, mat, dir);
+
+  // find support point on unit sphere: scale dir by ellipsoid sizes
+  tmp[0] = local_dir[0] * size[0];
+  tmp[1] = local_dir[1] * size[1];
+  tmp[2] = local_dir[2] * size[2];
+
+  mjtNum norm = mju_sqrt(tmp[0]*tmp[0] + tmp[1]*tmp[1] + tmp[2]*tmp[2]);
+
+  // try normalizing and transform to ellipsoid
+  if (norm < mjMINVAL) {
+    tmp[0] = size[0];
+    tmp[1] = 0;
+    tmp[2] = 0;
+  } else {
+    mjtNum norm_inv = 1/norm;
+    tmp[0] *= norm_inv * size[0];
+    tmp[1] *= norm_inv * size[1];
+    tmp[2] *= norm_inv * size[2];
+  }
+
+  // transform result to global frame
+  localToGlobal(res, mat, tmp, pos);
+}
+
+
+
+// cylinder support function
+static void mjc_cylinderSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
+  const mjModel* m = obj->model;
+  const mjData* d = obj->data;
+
+  // cylinder data
+  int i = 3*obj->geom;
+  const mjtNum* mat = d->geom_xmat + 3*i;
+  const mjtNum* pos = d->geom_xpos + i;
+  const mjtNum* size = m->geom_size + i;
+
+  // rotate dir to geom local frame
+  mjtNum local_dir[3], tmp[3];
+  mulMatTVec3(local_dir, mat, dir);
+
+  mjtNum n = local_dir[0]*local_dir[0] + local_dir[1]*local_dir[1];
+  if (n > mjMINVAL*mjMINVAL) {
+    n = size[0] / mju_sqrt(n);
+    tmp[0] = local_dir[0] * n;
+    tmp[1] = local_dir[1] * n;
+  } else {
+    tmp[0] = tmp[1] = 0;
+  }
+
+  // set result in Z direction
+  tmp[2] = mju_sign(local_dir[2]) * size[1];
+
+  // transform result to global frame
+  localToGlobal(res, mat, tmp, pos);
+}
+
+
+
+// box support function
+static void mjc_boxSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
+  const mjModel* m = obj->model;
+  const mjData* d = obj->data;
+
+  // box data
+  int i = 3*obj->geom;
+  const mjtNum* mat = d->geom_xmat + 3*i;
+  const mjtNum* pos = d->geom_xpos + i;
+  const mjtNum* size = m->geom_size + i;
+
+  // rotate dir to geom local frame
+  mjtNum local_dir[3], tmp[3];
+  mulMatTVec3(local_dir, mat, dir);
+
+  tmp[0] = mju_sign(local_dir[0]) * size[0];
+  tmp[1] = mju_sign(local_dir[1]) * size[1];
+  tmp[2] = mju_sign(local_dir[2]) * size[2];
+
+  // transform result to global frame
+  localToGlobal(res, mat, tmp, pos);
+}
+
+
+
+// mesh support function via exhaustive search
+static void mjc_meshSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
+  const mjModel* m = obj->model;
+  const mjData* d = obj->data;
+
+  // mesh data
+  int g = obj->geom;
+  const mjtNum* mat = d->geom_xmat+9*g;
+  const mjtNum* pos = d->geom_xpos+3*g;
+  float* verts = m->mesh_vert + 3*m->mesh_vertadr[m->geom_dataid[g]];
+  int nverts = m->mesh_vertnum[m->geom_dataid[g]];
+
+  mjtNum local_dir[3];
+  mulMatTVec3(local_dir, mat, dir);
+
+  mjtNum tmp = -1E+10;
+  int ibest = -1;
+  if (obj->meshindex >= 0) {
+    ibest = obj->meshindex;
+    tmp = local_dir[0] * (mjtNum)verts[3*ibest + 0] +
+          local_dir[1] * (mjtNum)verts[3*ibest + 1] +
+          local_dir[2] * (mjtNum)verts[3*ibest + 2];
+  }
+
+  // search all vertices, find best
+  for (int i=0; i < nverts; i++) {
+    mjtNum vdot = local_dir[0] * (mjtNum)verts[3*i + 0] +
+                  local_dir[1] * (mjtNum)verts[3*i + 1] +
+                  local_dir[2] * (mjtNum)verts[3*i + 2];
+
+    // update best
+    if (vdot > tmp) {
+      tmp = vdot;
+      ibest = i;
+    }
+  }
+
+  // record best vertex index
+  obj->meshindex = ibest;
+
+  local_dir[0] = (mjtNum)verts[3*ibest + 0];
+  local_dir[1] = (mjtNum)verts[3*ibest + 1];
+  local_dir[2] = (mjtNum)verts[3*ibest + 2];
+
+  // transform result to global frame
+  localToGlobal(res, mat, local_dir, pos);
+}
+
+
+
+// mesh support function via hill climbing
+static void mjc_hillclimbSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
+  const mjModel* m = obj->model;
+  const mjData* d = obj->data;
+
+  // get mesh info
+  int g = obj->geom;
+  int graphadr = m->mesh_graphadr[m->geom_dataid[g]];
+  int numvert = m->mesh_graph[graphadr];
+  int* vert_edgeadr = m->mesh_graph + graphadr + 2;
+  int* vert_globalid = m->mesh_graph + graphadr + 2 + numvert;
+  int* edge_localid = m->mesh_graph + graphadr + 2 + 2*numvert;
+  float* verts = m->mesh_vert + 3*m->mesh_vertadr[m->geom_dataid[g]];
+  const mjtNum* mat = d->geom_xmat + 9*g;
+  const mjtNum* pos = d->geom_xpos+3*g;
+
+  // rotate dir to geom local frame
+  mjtNum local_dir[3];
+  mulMatTVec3(local_dir, mat, dir);
+
+  mjtNum tmp = -1E+10;
+  int ibest= -1, prev = -1;
+  // hill-climb until no change
+  do {
+    prev = ibest;
+    for (int i = vert_edgeadr[ibest]; edge_localid[i] >= 0; i++) {
+      int idx = 3*vert_globalid[edge_localid[i]];
+      mjtNum vdot = local_dir[0] * (mjtNum)verts[idx + 0] +
+                    local_dir[1] * (mjtNum)verts[idx + 1] +
+                    local_dir[2] * (mjtNum)verts[idx + 2];
+      if (vdot > tmp) {
+        tmp = vdot;
+        ibest = edge_localid[i];  // update best
+      }
+    }
+  } while (ibest != prev);
+
+  // record best vertex index (local id)
+  obj->meshindex = ibest;
+
+  // map best index to globalid
+  ibest = vert_globalid[ibest];
+
+  // sanity check, SHOULD NOT OCCUR
+  if (ibest < 0) {
+    mju_warning("mesh_support could not find support vertex");
+    mju_zero3(res);
+  } else {
+    local_dir[0] = (mjtNum)verts[3*ibest + 0];
+    local_dir[1] = (mjtNum)verts[3*ibest + 1];
+    local_dir[2] = (mjtNum)verts[3*ibest + 2];
+  }
+
+  // transform result to global frame
+  localToGlobal(res, mat, local_dir, pos);
+}
+
+
+
+// prism support function
+static void mjc_prism_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
+  int istart, ibest;
+  mjtNum best, tmp;
+
+  // find best vertex in halfspace determined by dir.z
+  istart = dir[2] < 0 ? 0 : 3;
+  ibest = istart;
+  best = mju_dot3(obj->prism[istart], dir);
+  for (int i=istart+1; i < istart+3; i++) {
+    if ((tmp = mju_dot3(obj->prism[i], dir)) > best) {
+      ibest = i;
+      best = tmp;
+    }
+  }
+
+  // copy best point
+  mju_copy3(res, obj->prism[ibest]);
+}
+
+
+
+// flex support function
+static void mjc_flexSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
+  const mjModel* m = obj->model;
+  const mjData* d = obj->data;
+  int f = obj->flex;
+  int dim = m->flex_dim[f];
+
+  // flex element
+  if (obj->elem >= 0) {
+    int e = obj->elem;
+    const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
+    const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f];
+
+    // find element vertex with largest projection along dir
+    mju_copy3(res, vert+3*edata[0]);
+    mjtNum best = mju_dot3(res, dir);
+    for (int i=1; i <= dim; i++) {
+      mjtNum dot = mju_dot3(vert+3*edata[i], dir);
+
+      // better vertex found: assign
+      if (dot > best) {
+        best = dot;
+        mju_copy3(res, vert+3*edata[i]);
+      }
+    }
+
+    // add radius and margin/2
+    mju_addToScl3(res, dir, m->flex_radius[f] + 0.5*obj->margin);
+    return;
+  }
+
+  // flex vertex
+  else {
+    const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + obj->vert);
+    mju_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*obj->margin);
+    return;
+  }
+}
+
+
+
+// libccd support function
+void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
+  mjCCDObj *obj = (mjCCDObj *)_obj;
+  mjtNum *res = vec->v;
+  const mjtNum *dir = _dir->v;
   const mjModel* m = obj->model;
   const mjData* d = obj->data;
   int g = obj->geom;
 
-  //-------------------------- flex element or vertex -----------------------------
   if (g < 0) {
     int f = obj->flex;
     int dim = m->flex_dim[f];
@@ -150,7 +501,6 @@ void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
     }
   }
 
-  //-------------------------- geom -------------------------------------------
   float* vertdata;
   int ibest, graphadr, numvert, change, locid;
   int *vert_edgeadr, *vert_globalid, *edge_localid;
@@ -314,6 +664,78 @@ void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
 
 
 
+// libccd prism support function
+static void mjccd_prism_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec) {
+  mjc_prism_support(vec->v, (mjCCDObj*) obj, dir->v);
+}
+
+// ------------------------------------------------------------------------------------------------
+
+// initialize a CCD object
+void mjc_initCCDObj(mjCCDObj* obj, const mjModel* m, const mjData* d, int g, mjtNum margin) {
+  obj->model = m;
+  obj->data = d;
+  obj->geom = g;
+  obj->margin = margin;
+  obj->center = mjc_center;
+  obj->meshindex = -1;
+  obj->flex = -1;
+  obj->elem = -1;
+  obj->vert = -1;
+  mju_zero4(obj->rotate);
+  obj->rotate[0] = 1;
+  if (g >= 0) {
+    obj->geom_type = m->geom_type[g];
+    switch ((mjtGeom) obj->geom_type) {
+    case mjGEOM_ELLIPSOID:
+      obj->support = mjc_ellipsoidSupport;
+      break;
+    case mjGEOM_MESH:
+    case mjGEOM_SDF:
+      if (m->mesh_graphadr[m->geom_dataid[g]] < 0 ||
+          m->mesh_vertnum[m->geom_dataid[g]] < mjMESH_HILLCLIMB_MIN) {
+        obj->support = mjc_meshSupport;
+      } else {
+        obj->support = mjc_hillclimbSupport;
+      }
+      break;
+    case mjGEOM_SPHERE:
+      obj->support = mjc_sphereSupport;
+      break;
+    case mjGEOM_CAPSULE:
+      obj->support = mjc_capsuleSupport;
+      break;
+    case mjGEOM_CYLINDER:
+      obj->support = mjc_cylinderSupport;
+      break;
+    case mjGEOM_BOX:
+      obj->support = mjc_boxSupport;
+      break;
+    case mjGEOM_HFIELD:
+      obj->center = mjc_prism_center;
+      obj->support = mjc_prism_support;
+      break;
+    default:
+      obj->support = NULL;
+      break;
+    }
+  } else {
+    obj->geom_type = mjGEOM_FLEX;
+    obj->support = mjc_flexSupport;
+  }
+}
+
+
+
+// set flex data for CCD object
+static void mjc_setCCDObjFlex(mjCCDObj* obj, int flex, int elem, int vert) {
+  obj->flex = flex;
+  obj->elem = elem;
+  obj->vert = vert;
+}
+
+
+
 // initialize CCD structure
 static void mjc_initCCD(ccd_t* ccd, const mjModel* m) {
   CCD_INIT(ccd);
@@ -395,25 +817,13 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
 // multi-point convex-convex collision, using libccd
 int mjc_Convex(const mjModel* m, const mjData* d,
                mjContact* con, int g1, int g2, mjtNum margin) {
-  ccd_t ccd;
+  // init ccd objects
-  mjCCDObj obj1 = {.model = m, .data = d,
+  mjCCDObj obj1, obj2;
-                   .geom = g1,
+  mjc_initCCDObj(&obj1, m, d, g1, margin);
-                   .geom_type = m->geom_type[g1],
+  mjc_initCCDObj(&obj2, m, d, g2, margin);
-                   .meshindex = -1, .flex = -1, .elem = -1, .vert = -1,
-                   .margin = margin,
-                   .rotate = {1, 0, 0, 0},
-                   .center = mjc_center,
-                   .support = mjc_support};
-  mjCCDObj obj2 = {.model = m, .data = d,
-                   .geom = g2,
-                   .geom_type = m->geom_type[g2],
-                   .meshindex = -1, .flex = -1, .elem = -1, .vert = -1,
-                   .margin = margin,
-                   .rotate = {1, 0, 0, 0},
-                   .center = mjc_center,
-                   .support = mjc_support};
 
-  // init ccd structure
+  // init libccd structure
+  ccd_t ccd;
   mjc_initCCD(&ccd, m);
   ccd.first_dir = ccdFirstDirDefault;
   ccd.center1 = mjccd_center;
@@ -539,13 +949,8 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
   mjGETINFO
   mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]};
   ccd_vec3_t dir, vec;
-  mjCCDObj obj = {.model = m, .data = d,
+  mjCCDObj obj;
-                  .geom = g2,
+  mjc_initCCDObj(&obj, m, d, g2, 0);
-                  .geom_type = m->geom_type[g2],
-                  .meshindex = -1, .flex = -1, .elem = -1, .vert = -1,
-                  .margin = 0,
-                  .rotate = {1, 0, 0, 0}};
-
   // get support point in -normal direction
   ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]);
   mjccd_support(&obj, &dir, &vec);
@@ -641,48 +1046,6 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
 
 //----------------------------  heightfield collisions ---------------------------------------------
 
-// prism support function
-static void mjc_prism_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
-  int istart, ibest;
-  mjtNum best, tmp;
-
-  // find best vertex in halfspace determined by dir.z
-  istart = dir[2] < 0 ? 0 : 3;
-  ibest = istart;
-  best = mju_dot3(obj->prism[istart], dir);
-  for (int i=istart+1; i < istart+3; i++) {
-    if ((tmp = mju_dot3(obj->prism[i], dir)) > best) {
-      ibest = i;
-      best = tmp;
-    }
-  }
-
-  // copy best point
-  mju_copy3(res, obj->prism[ibest]);
-}
-
-// ccd prism support function
-static void mjccd_prism_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec) {
-  mjc_prism_support(vec->v, (mjCCDObj*) obj, dir->v);
-}
-
-
-// prism center function
-static void mjc_prism_center(mjtNum res[3], const mjCCDObj* obj) {
-  // compute mean
-  mju_zero3(res);
-  for (int i=0; i < 6; i++) {
-    mju_addTo3(res, obj->prism[i]);
-  }
-  mju_scl3(res, res, 1.0/6.0);
-}
-
-// ccd prism center function
-static void mjccd_prism_center(const void *obj, ccd_vec3_t *center) {
-  mjc_prism_center(center->v, (const mjCCDObj*) obj);
-}
-
-
 // ccd prism first dir
 static void prism_firstdir(const void* o1, const void* o2, ccd_vec3_t *vec) {
   ccdVec3Set(vec, 0, 0, 1);
@@ -719,26 +1082,14 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
   int ncol = m->hfield_ncol[hid];
   int dr[2], cnt, rmin, rmax, cmin, cmax;
   const float* data = m->hfield_data + m->hfield_adr[hid];
-  mjCCDObj obj1 = {.model = m, .data = d,
-                   .geom = -1,
-                   .geom_type = mjGEOM_HFIELD,
-                   .meshindex = -1, .flex = -1, .elem = -1, .vert = -1,
-                   .margin = 0,
-                   .rotate = {1, 0, 0, 0},
-                   .center = mjc_prism_center,
-                   .support = mjc_prism_support};
 
   // ccd-related
+  mjCCDObj obj1, obj2;
+  mjc_initCCDObj(&obj1, m, d, g1, 0);
+  mjc_initCCDObj(&obj2, m, d, g2, 0);
+
   ccd_vec3_t dirccd, vecccd;
   ccd_real_t depth;
-  mjCCDObj obj2 = {.model = m, .data = d,
-                   .geom = g2,
-                   .geom_type = m->geom_type[g2],
-                   .meshindex = -1, .flex = -1, .elem = -1, .vert = -1,
-                   .margin = 0,
-                   .rotate = {1, 0, 0, 0},
-                   .center = mjc_center,
-                   .support = mjc_support};
   ccd_t ccd;
 
   // point size1 to hfield size instead of geom1 size
@@ -1171,31 +1522,14 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
 // geom-elem or elem-elem or vert-elem convex collision using ccd
 int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
                    int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) {
-  ccd_t ccd;
+  mjCCDObj obj1, obj2;
-  mjCCDObj obj1 = {.model = m, .data = d,
+  mjc_initCCDObj(&obj1, m, d, g1, margin);
-                   .geom = g1,
+  mjc_initCCDObj(&obj2, m, d, -1, margin);
-                   .geom_type = (g1 >= 0) ? m->geom_type[g1] : mjGEOM_FLEX,
+  mjc_setCCDObjFlex(&obj1, f1, e1, v1);
-                   .meshindex = -1,
+  mjc_setCCDObjFlex(&obj2, f2, e2, -1);
-                   .flex = f1,
-                   .elem = e1,
-                   .vert = v1,
-                   .margin = margin,
-                   .rotate = {1, 0, 0, 0},
-                   .center = mjc_center,
-                   .support = mjc_support};
-  mjCCDObj obj2 = {.model = m, .data = d,
-                   .geom = -1,
-                   .geom_type = mjGEOM_FLEX,
-                   .meshindex = -1,
-                   .flex = f2,
-                   .elem = e2,
-                   .vert = -1,
-                   .margin = margin,
-                   .rotate = {1, 0, 0, 0},
-                   .center = mjc_center,
-                   .support = mjc_support};
 
-  // init ccd structure
+  // init libccd structure
+  ccd_t ccd;
   mjc_initCCD(&ccd, m);
   ccd.first_dir = ccdFirstDirDefault;
   ccd.center1 = mjccd_center;
@@ -1242,17 +1576,9 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
   // ccd-related
   ccd_vec3_t dirccd, vecccd;
   ccd_real_t depth;
-  mjCCDObj obj2 = {.model = m, .data = d,
+  mjCCDObj obj2;
-                   .geom = -1,
+  mjc_initCCDObj(&obj2, m, d, -1, margin);
-                   .geom_type = mjGEOM_FLEX,
+  mjc_setCCDObjFlex(&obj2, f, e, -1);
-                   .meshindex = -1,
-                   .flex = f,
-                   .elem = e,
-                   .vert = -1,
-                   .margin = margin,
-                   .rotate = {1, 0, 0, 0},
-                   .center = mjc_center,
-                   .support = mjc_support};
   ccd_t ccd;
 
   //------------------------------------- AABB computation, box-box test

src/engine/engine_collision_convex.h

@@ -36,6 +36,9 @@
           mjtNum* mat2  = d->geom_xmat + 9*g2;
 // mjc_ConvexHField modifies and then restores pos2 and mat2
 
+// minimum number of vertices to use hill-climbing in mesh support
+#define mjMESH_HILLCLIMB_MIN 10
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -58,14 +61,17 @@ struct _mjCCDObj {
 };
 typedef struct _mjCCDObj mjCCDObj;
 
-// support function for convex collision algorithms
+// initialize a CCD object
-MJAPI void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]);
+MJAPI void mjc_initCCDObj(mjCCDObj* obj, const mjModel* m, const mjData* d, int g, mjtNum margin);
 
 // center function for convex collision algorithms
 MJAPI void mjc_center(mjtNum res[3], const mjCCDObj *obj);
 
-// ccd support function
+// libccd center function
-void mjccd_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec);
+MJAPI void mjccd_center(const void *obj, ccd_vec3_t *center);
+
+// libccd support function
+MJAPI void mjccd_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec);
 
 // pairwise geom collision functions using ccd
 int mjc_PlaneConvex   (const mjModel* m, const mjData* d,

src/engine/engine_collision_gjk.c

@@ -37,8 +37,8 @@ static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], const
 static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const mjtNum s3[3]);
 static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]);
 
-// helper function to compute the support point in the Minkowski difference
+// helper function to compute the support point for EPA
-static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
+static void epaSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
                     const mjtNum d[3], mjtNum dnorm);
 
 // support function tweaked for GJK by taking kth iteration point as input and setting both
@@ -68,7 +68,7 @@ typedef struct {
   int nfaces;        // number of faces
   int maxfaces;      // max number of faces that can be stored in polytope
   Face** map;        // linear map storing faces
-  int nmap;         // number of faces in map
+  int nmap;          // number of faces in map
 } Polytope;
 
 // copies a vertex into the polytope and returns its index
@@ -230,7 +230,32 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
 
   status->gjk_iterations = k;
   status->nsimplex = n;
-  return mju_norm3(x_k);
+  status->gjk_dist = mju_norm3(x_k);
+  return status->gjk_dist;
+}
+
+
+
+// computes the support point in obj1 and obj2 for Minkowski difference
+static inline void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
+                           const mjtNum dir[3], const mjtNum dir_neg[3]) {
+  // obj1
+  obj1->support(s1, obj1, dir);
+  if (obj1->margin > 0 && obj1->geom >= 0) {
+    mjtNum margin = 0.5 * obj1->margin;
+    s1[0] += dir[0] * margin;
+    s1[1] += dir[1] * margin;
+    s1[2] += dir[2] * margin;
+  }
+
+  // obj2
+  obj2->support(s2, obj2, dir_neg);
+  if (obj2->margin > 0 && obj2->geom >= 0) {
+    mjtNum margin = 0.5 * obj2->margin;
+    s2[0] += dir_neg[0] * margin;
+    s2[1] += dir_neg[1] * margin;
+    s2[2] += dir_neg[2] * margin;
+  }
 }
 
 
@@ -244,15 +269,14 @@ static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj
   scl3(dir, dir_neg, -1);
 
   // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
-  obj1->support(s1, obj1, dir);
+  support(s1, s2, obj1, obj2, dir, dir_neg);
-  obj2->support(s2, obj2, dir_neg);
 }
 
 
 
 // helper function to compute the support point in the Minkowski difference
-static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
+static void epaSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
-                    const mjtNum d[3], mjtNum dnorm) {
+                       const mjtNum d[3], mjtNum dnorm) {
   mjtNum dir[3], dir_neg[3];
 
   // mjc_support assumes a normalized direction
@@ -270,21 +294,17 @@ static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
   }
 
   // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
-  obj1->support(s1, obj1, dir);
+  support(s1, s2, obj1, obj2, dir, dir_neg);
-  obj2->support(s2, obj2, dir_neg);
 }
 
 
 
 // helper function to compute the support point in the Minkowski difference (without normalization)
-// TODO(kylebayes): combine support functions
+static void gjkIntersectSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
-void support2(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum dir[3]) {
+                                const mjtNum dir[3]) {
-  mjtNum dir_neg[3];
+  mjtNum dir_neg[3] = {-dir[0], -dir[1], -dir[2]};
-  scl3(dir_neg, dir, -1);
-
   // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
-  obj1->support(s1, obj1, dir);
+  support(s1, s2, obj1, obj2, dir, dir_neg);
-  obj2->support(s2, obj2, dir_neg);
 }
 
 
@@ -343,7 +363,7 @@ static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj
     }
 
     // replace worst vertex (farthest from origin) with new candidate
-    support2(simplex1 + s[index], simplex2 + s[index], obj1, obj2, normals + 3*index);
+    gjkIntersectSupport(simplex1 + s[index], simplex2 + s[index], obj1, obj2, normals + 3*index);
     sub3(simplex + s[index], simplex1 + s[index], simplex2 + s[index]);
 
     // found origin outside the Minkowski difference (return no collision)
@@ -815,15 +835,15 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
 
 
   mjtNum v3a[3], v3b[3], v3[3];
-  support(v3a, v3b, obj1, obj2, d1, mju_norm3(d1));
+  epaSupport(v3a, v3b, obj1, obj2, d1, mju_norm3(d1));
   sub3(v3, v3a, v3b);
 
   mjtNum v4a[3], v4b[3], v4[3];
-  support(v4a, v4b, obj1, obj2, d2, mju_norm3(d2));
+  epaSupport(v4a, v4b, obj1, obj2, d2, mju_norm3(d2));
   sub3(v4, v4a, v4b);
 
   mjtNum v5a[3], v5b[3], v5[3];
-  support(v5a, v5b, obj1, obj2, d3, mju_norm3(d3));
+  epaSupport(v5a, v5b, obj1, obj2, d3, mju_norm3(d3));
   sub3(v5, v5a, v5b);
 
   // check that all six faces are valid triangles (not collinear)
@@ -930,10 +950,9 @@ static int triPointIntersect(const mjtNum v1[3], const mjtNum v2[3], const mjtNu
 // creates a polytope from a 2-simplex (returns 0 if polytope can be created)
 static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
   // get vertices of simplex from GJK
-  mjtNum v1[3], v2[3], v3[3];
+  const mjtNum *v1 = status->simplex,
-  sub3(v1, status->simplex1 + 0, status->simplex2 + 0);
+               *v2 = status->simplex + 3,
-  sub3(v2, status->simplex1 + 3, status->simplex2 + 3);
+               *v3 = status->simplex + 6;
-  sub3(v3, status->simplex1 + 6, status->simplex2 + 6);
 
   // get normals in both directions
   mjtNum diff1[3], diff2[3], n[3], n_neg[3];
@@ -950,7 +969,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
 
   // get 4th vertex in n direction
   mjtNum v4a[3], v4b[3], v4[3];
-  support(v4a, v4b, obj1, obj2, n, n_norm);
+  epaSupport(v4a, v4b, obj1, obj2, n, n_norm);
   sub3(v4, v4a, v4b);
 
   // check that v4 is not contained in the 2-simplex
@@ -960,7 +979,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
 
   // get 5th vertex in -n direction
   mjtNum v5a[3], v5b[3], v5[3];
-  support(v5a, v5b, obj1, obj2, n_neg, n_norm);
+  epaSupport(v5a, v5b, obj1, obj2, n_neg, n_norm);
   sub3(v5, v5a, v5b);
 
   // check that v5 is not contained in the 2-simplex
@@ -974,9 +993,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
   // TODO(kylebayes): It's possible for GJK to return a 2-simplex with the origin not contained in
   // it but within tolerance from it. In that case the hexahedron could possibly be constructed
   // that doesn't contain the origin, but nonetheless there is penetration depth.
-  mjtNum dir[3];
+  if (status->gjk_dist > 10*mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) {
-  sub3(dir, status->x1, status->x2);
-  if (mju_norm3(dir) > mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) {
     return 7;
   }
 
@@ -1229,7 +1246,7 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o
 
     // compute support point w from the closest face's normal
     mjtNum w1[3], w2[3], w[3];
-    support(w1, w2, obj1, obj2, face->v, dist);
+    epaSupport(w1, w2, obj1, obj2, face->v, dist);
     sub3(w, w1, w2);
     mjtNum next_dist = dot3(face->v, w) / dist;
     if (next_dist - dist < tolerance) {

src/engine/engine_collision_gjk.h

@@ -44,6 +44,7 @@ struct _mjCCDStatus {
   int has_distances;          // set to true if attempted to recover distance info
 
   // statistics for debugging purposes
+  mjtNum gjk_dist;             // the distance returned by GJK
   int gjk_iterations;         // number of iterations that GJK ran
   int epa_iterations;         // number of iterations that EPA ran (negative if EPA did not run)
   mjtNum simplex1[12];        // the simplex that GJK returned for obj1

test/engine/engine_collision_gjk_test.cc

@@ -55,16 +55,6 @@ constexpr char kEllipoid[] = R"(
   </keyframe>
 </mujoco>)";
 
-// ccd center function
-void mjccd_center(const void *obj, ccd_vec3_t *center) {
-  mjc_center(center->v, (const mjCCDObj*) obj);
-}
-
-// ccd support function
-void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
-  mjc_support(vec->v, (mjCCDObj*) obj, _dir->v);
-}
-
 mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
                 mjtNum x2[3]) {
   mjCCDConfig config;
@@ -76,10 +66,10 @@ mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
   config.contacts = 0;   // no geom contacts needed
   config.distances = 1;
 
-  mjCCDObj obj1 = {m, d, g1, m->geom_type[g1], -1, -1, -1, -1, 0, {1, 0, 0, 0},
+  mjCCDObj obj1, obj2;
-                   mjc_center, mjc_support};
+  mjc_initCCDObj(&obj1, m, d, g1, 0);
-  mjCCDObj obj2 = {m, d, g2, m->geom_type[g2], -1, -1, -1, -1, 0, {1, 0, 0, 0},
+  mjc_initCCDObj(&obj2, m, d, g2, 0);
-                   mjc_center, mjc_support};
+
   mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2);
   if (x1 != nullptr) mju_copy3(x1, status.x1);
   if (x2 != nullptr) mju_copy3(x2, status.x2);
@@ -121,10 +111,10 @@ int PenetrationWrapper(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd,
 mjtNum Penetration(mjModel* m, mjData* d, int g1, int g2,
                    mjtNum dir[3] = nullptr, mjtNum pos[3] = nullptr,
                    mjtNum margin = 0) {
-  mjCCDObj obj1 = {m, d, g1, m->geom_type[g1], -1, -1, -1, -1, margin,
+  mjCCDObj obj1, obj2;
-                   {1, 0, 0, 0}, mjc_center, mjc_support};
+  mjc_initCCDObj(&obj1, m, d, g1, margin);
-  mjCCDObj obj2 = {m, d, g2, m->geom_type[g2], -1, -1, -1, -1, margin,
+  mjc_initCCDObj(&obj2, m, d, g2, margin);
-                   {1, 0, 0, 0}, mjc_center, mjc_support};
+
   ccd_t ccd;
   // CCD_INIT(&ccd);  // uncomment to run ccdMPRPenetration
   ccd.mpr_tolerance = kTolerance;
@@ -423,7 +413,7 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) {
   int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
   mjtNum dist = Penetration(model, data, geom1, geom2, nullptr, nullptr, 15);
 
-  EXPECT_LT(dist, 0);
+  EXPECT_NEAR(dist, -14.245732934582151, kTolerance);
   mj_deleteData(data);
   mj_deleteModel(model);
 }