Introduce mjPreContact, a minimal struct passed into the collision functions.

PiperOrigin-RevId: 918533795
Change-Id: I2b5af05c1479b25d5c2cfdc690321a26fce6ede6

src/engine/engine_collision_box.c

@@ -20,24 +20,18 @@
 #include "engine/engine_util_misc.h"
 
 // hard-clamp vector to range [-limit(i), +limit(i)]
-static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n)
-{
-  int i;
-
-  // loop over active limits
-  for (i = 0; i < n; i++) {
+static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n) {
+  for (int i = 0; i < n; i++) {
+    // loop over active limits
     if (limit[i] > 0) {
-      if (vec[i] < -limit[i])
-        vec[i] = -limit[i];
-      else if (vec[i] > limit[i])
-        vec[i] = limit[i];
+      vec[i] = mju_clip(vec[i], -limit[i], limit[i]);
     }
   }
 }
 
 
 // raw sphere : box
-int mjraw_SphereBox(mjContact* con, mjtNum margin,
+int mjraw_SphereBox(mjPreContact* con, mjtNum margin,
                     const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
                     const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
   int i, k;
@@ -74,29 +68,32 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin,
 
     mji_copy3(pos, center);
     mji_addToScl3(pos, nearest, (size1[0] - closest) / 2);
-    mji_mulMatVec3(con[0].frame, mat2, nearest);
+    mji_mulMatVec3(con[0].normal, mat2, nearest);
     dist = -closest;
   } else {
     mji_addToScl3(deepest, tmp, size1[0]);
     mju_zero3(pos);
     mji_addToScl3(pos, clamped, 0.5);
     mji_addToScl3(pos, deepest, 0.5);
-    mji_mulMatVec3(con[0].frame, mat2, tmp);
+    mji_mulMatVec3(con[0].normal, mat2, tmp);
   }
 
   mji_mulMatVec3(tmp, mat2, pos);
   mji_add3(con[0].pos, tmp, pos2);
   con[0].dist = dist - size1[0];
-  mju_zero3(con[0].frame + 3);
-
+  mji_zero3(con[0].tangent);
   return 1;
 }
 
 
 // sphere : box
-int mjc_SphereBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
-  mjGETINFO;
-
+int mjc_SphereBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) {
+  const mjtNum* pos1  = d->geom_xpos + 3*g1;
+  const mjtNum* mat1  = d->geom_xmat + 9*g1;
+  const mjtNum* size1 = m->geom_size + 3*g1;
+  const mjtNum* pos2  = d->geom_xpos + 3*g2;
+  const mjtNum* mat2  = d->geom_xmat + 9*g2;
+  const mjtNum* size2 = m->geom_size + 3*g2;
   return mjraw_SphereBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
 }
 
@@ -115,9 +112,10 @@ int mjc_SphereBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, m
 */
 
 // raw capsule : box
-int mjraw_CapsuleBox(mjContact* con, mjtNum margin,
+int mjraw_CapsuleBox(mjPreContact* con, mjtNum margin,
                      const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
-                     const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
+                     const mjtNum* pos2, const mjtNum* mat2,
+                     const mjtNum* size2) {
   mjtNum tmp1[3], tmp2[3], tmp3[3], halfaxis[3], axis[3], dif[3];
   mjtNum pos[3];          // position of capsule in box-local frame
 
@@ -148,7 +146,7 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin,
   secondpos = -4;  // initialize to no 2nd contact (valid values are between -1 and 1)
 
   mji_sub3(tmp1, pos1, pos2);       // bring capsule to box-local frame (center's box is at (0,0,0))
-  mji_mulMatTVec3(pos, mat2, tmp1);  // and axis parralel to world
+  mji_mulMatTVec3(pos, mat2, tmp1);  // and axis parallel to world
 
   tmp1[0] = mat1[2];  // capsule's axis
   tmp1[1] = mat1[5];
@@ -591,21 +589,25 @@ skip:
 
 
 // capsule : box
-int mjc_CapsuleBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
-  mjGETINFO
+int mjc_CapsuleBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) {
+  const mjtNum* pos1  = d->geom_xpos + 3*g1;
+  const mjtNum* pos2  = d->geom_xpos + 3*g2;
+  const mjtNum* mat1  = d->geom_xmat + 9*g1;
+  const mjtNum* mat2  = d->geom_xmat + 9*g2;
+  const mjtNum* size1 = m->geom_size + 3*g1;
+  const mjtNum* size2 = m->geom_size + 3*g2;
   return mjraw_CapsuleBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
 }
 
 
 // internal box : box
 static inline
-int _boxbox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, mjtNum margin)
-{
+int _boxbox(const mjModel* M, const mjData* D, mjPreContact* con, int g1, int g2, mjtNum margin) {
   const mjtNum* pos1 = D->geom_xpos + 3 * g1;
-  const mjtNum* mat1 = D->geom_xmat + 9 * g1;
-  const mjtNum* size1 = M->geom_size + 3 * g1;
   const mjtNum* pos2 = D->geom_xpos + 3 * g2;
+  const mjtNum* mat1 = D->geom_xmat + 9 * g1;
   const mjtNum* mat2 = D->geom_xmat + 9 * g2;
+  const mjtNum* size1 = M->geom_size + 3 * g1;
   const mjtNum* size2 = M->geom_size + 3 * g2;
 
   mjtNum pos12[3], pos21[3], rot[9], rott[9], rotabs[9], rottabs[9], tmp1[3], tmp2[3], plen1[3],
@@ -883,8 +885,10 @@ int _boxbox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, m
           if (mju_abs(c2) > ss[1 - q])
             continue;
 
-          mji_copy3(points[n], lines[i]);
-          mji_addToScl3(points[n++], lines[i] + 3, c1);
+          if (n < mjMAXCONPAIR) {
+            mji_copy3(points[n], lines[i]);
+            mji_addToScl3(points[n++], lines[i] + 3, c1);
+          }
         }
       }
     }
@@ -911,10 +915,12 @@ int _boxbox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, m
       if (u <= 0 || v <= 0 || u >= 1 || v >= 1)
         continue;
 
-      points[n][0] = llx;
-      points[n][1] = lly;
-      points[n][2] = (pts[0][2] + u * pts[1][2] + v * pts[2][2]);
-      n++;
+      if (n < mjMAXCONPAIR) {
+        points[n][0] = llx;
+        points[n][1] = lly;
+        points[n][2] = (pts[0][2] + u * pts[1][2] + v * pts[2][2]);
+        n++;
+      }
     }
   }
 
@@ -929,7 +935,9 @@ int _boxbox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, m
       if (tmp1[1] <= -ly || tmp1[1] >= ly)
         continue;
 
-    mji_copy3(points[n++], tmp1);
+    if (n < mjMAXCONPAIR) {
+      mji_copy3(points[n++], tmp1);
+    }
   }
 
 
@@ -955,22 +963,23 @@ int _boxbox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, m
   tmp2[1] = (q2 ? -1 : 1) * r[5];
   tmp2[2] = (q2 ? -1 : 1) * r[8];
 
-  mji_copy3(con[0].frame, tmp2);
-  mju_zero3(con[0].frame + 3);
+  mji_copy3(con[0].normal, tmp2);
+  mji_zero3(con[0].tangent);
 
 
 
 
-  for (i = 0; i < n; i++)
-  {
+  for (i = 0; i < n; i++) {
     con[i].dist = 2 * points[i][2];
     points[i][2] += hz;
 
     mji_mulMatVec3(tmp2, r, points[i]);
     mji_add3(con[i].pos, tmp2, p);
 
-    if (i)
-      mji_copy6(con[i].frame, con[0].frame);
+    if (i) {
+      mji_copy3(con[i].normal, con[0].normal);
+      mji_zero3(con[i].tangent);
+    }
   }
 
 
@@ -1315,8 +1324,8 @@ edgeedge:
 
   mji_mulMatVec3(tmp1, r, rnorm);
 
-  mji_scl3(con[0].frame, tmp1, in ? -1 : 1);
-  mju_zero3(con[0].frame + 3);
+  mji_scl3(con[0].normal, tmp1, in ? -1 : 1);
+  mji_zero3(con[0].tangent);
 
 
   for (i = 0; i < n; i++) {
@@ -1327,7 +1336,8 @@ edgeedge:
 
     mji_add3(con[i].pos, tmp2, pos1);
 
-    mji_copy6(con[i].frame, con[0].frame);
+    mji_copy3(con[i].normal, con[0].normal);
+    mji_zero3(con[i].tangent);
   }
 
   return n;
@@ -1338,13 +1348,13 @@ edgeedge:
 
 
 // box : box
-int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
-  int num = _boxbox(m, d, con, g1, g2, margin);
+int mjc_BoxBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) {
+  mjPreContact tmp[mjMAXCONPAIR];
+  int num = _boxbox(m, d, tmp, g1, g2, margin);
+
+  // -1: bad, 0: good
+  int dupe[mjMAXCONPAIR] = {0};
 
-  // use dim field to mark: -1: bad, 0: good
-  for (int i=0; i < num; i++) {
-    con[i].dim = 0;
-  }
 
   // get box info
   const mjtNum* pos1 =  d->geom_xpos + 3 * g1;
@@ -1364,28 +1374,28 @@ int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN
     static mjtNum kRemoveRatio = 1.01;
 
     // is the contact outside: 1, inside: -1, within the removal width: 0
-    int out1 = mju_outsideBox(con[i].pos, pos1, mat1, sz1, kRemoveRatio);
-    int out2 = mju_outsideBox(con[i].pos, pos2, mat2, sz2, kRemoveRatio);
+    int out1 = mju_outsideBox(tmp[i].pos, pos1, mat1, sz1, kRemoveRatio);
+    int out2 = mju_outsideBox(tmp[i].pos, pos2, mat2, sz2, kRemoveRatio);
 
     // mark as bad if outside one box and not inside the other box
     if ((out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1)) {
-      con[i].dim = -1;
+      dupe[i] = -1;
     }
   }
 
   // find duplicates
   for (int i=0; i < num-1; i++) {
-    if (con[i].dim == -1) {
+    if (dupe[i] == -1) {
       continue;  // already marked bad: skip
     }
     for (int j=i+1; j < num; j++) {
-      if (con[j].dim == -1) {
+      if (dupe[j] == -1) {
         continue;  // already marked bad: skip
       }
-      if (con[i].pos[0] == con[j].pos[0] &&
-          con[i].pos[1] == con[j].pos[1] &&
-          con[i].pos[2] == con[j].pos[2]) {
-        con[i].dim = -1;
+      if (tmp[i].pos[0] == tmp[j].pos[0] &&
+          tmp[i].pos[1] == tmp[j].pos[1] &&
+          tmp[i].pos[2] == tmp[j].pos[2]) {
+        dupe[i] = -1;
         break;
       }
     }
@@ -1394,14 +1404,8 @@ int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN
   // consolidate good
   int i = 0;
   for (int j=0; j < num; j++) {
-    // good: maybe copy
-    if (con[j].dim == 0) {
-      // different: copy
-      if (i < j) {
-        con[i] = con[j];
-      }
-
-      // advance either way
+    if (dupe[j] == 0) {
+      con[i] = tmp[j];
       i++;
     }
   }

src/engine/engine_collision_convex.c

@@ -41,7 +41,7 @@ static void prism_firstdir(const void* o1, const void* o2, ccd_vec3_t *vec) {
 
 // wrapper around libccd; returns number of collisions found
 static inline int _libccd_wrapper(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2,
-                                  mjContact* con, mjtNum margin) {
+                                  mjPreContact* con, mjtNum margin) {
   ccd_t ccd;
   CCD_INIT(&ccd);
   ccd.mpr_tolerance = m->opt.ccd_tolerance;
@@ -64,10 +64,10 @@ static inline int _libccd_wrapper(const mjModel* m, mjCCDObj* obj1, mjCCDObj* ob
     if (ccdVec3Eq(&ccd_dir, ccd_vec3_origin)) {
       return 0;
     }
-    con->dist = margin - ccd_depth;
-    mji_copy3(con->frame, ccd_dir.v);
-    mji_copy3(con->pos, ccd_pos.v);
-    mji_zero3(con->frame + 3);
+    con[0].dist = margin - ccd_depth;
+    mji_copy3(con[0].normal, ccd_dir.v);
+    mji_copy3(con[0].pos, ccd_pos.v);
+    mji_zero3(con[0].tangent);
     return 1;
   }
   return 0;
@@ -76,7 +76,7 @@ static inline int _libccd_wrapper(const mjModel* m, mjCCDObj* obj1, mjCCDObj* ob
 
 // find penetration info between two geoms; returns number of collisions found
 static int mjc_penetration(const mjModel* m, mjData* d, mjCCDObj* obj1, mjCCDObj* obj2,
-                           mjContact* con, int ncon, mjtNum margin) {
+                           mjPreContact* con, int ncon, mjtNum margin) {
   if (mjDISABLED(mjDSBL_NATIVECCD)) {
     return _libccd_wrapper(m, obj1, obj2, con, margin);
   }
@@ -97,14 +97,14 @@ static int mjc_penetration(const mjModel* m, mjData* d, mjCCDObj* obj1, mjCCDObj
   if ((dist = mjc_ccd(&config, &status, obj1, obj2)) < 0) {
     mj_freeStack(d);
     int nwitness = status.nx;
-    for (int i = 0; i < nwitness; i++, con++) {
-      con->dist = margin + dist;
-      con->pos[0] = 0.5*(status.x1[3*i + 0] + status.x2[3*i + 0]);
-      con->pos[1] = 0.5*(status.x1[3*i + 1] + status.x2[3*i + 1]);
-      con->pos[2] = 0.5*(status.x1[3*i + 2] + status.x2[3*i + 2]);
-      mji_sub3(con->frame, status.x1 + 3*i, status.x2 + 3*i);
-      mju_normalize3(con->frame);
-      mji_zero3(con->frame + 3);
+    for (int i = 0; i < nwitness; i++) {
+      con[i].dist = margin + dist;
+      con[i].pos[0] = 0.5*(status.x1[3*i + 0] + status.x2[3*i + 0]);
+      con[i].pos[1] = 0.5*(status.x1[3*i + 1] + status.x2[3*i + 1]);
+      con[i].pos[2] = 0.5*(status.x1[3*i + 2] + status.x2[3*i + 2]);
+      mji_sub3(con[i].normal, status.x1 + 3*i, status.x2 + 3*i);
+      mju_normalize3(con[i].normal);
+      mji_zero3(con[i].tangent);
     }
     return nwitness;
   }
@@ -780,9 +780,10 @@ static void mjc_setCCDObjFlex(mjCCDObj* obj, int flex, int elem, int vert) {
 
 
 // compare new contact to previous contacts, return 1 if it is far from all of them
-static int mjc_isDistinctContact(mjContact* con, int ncon, mjtNum tolerance) {
+static int mjc_isDistinctContact(const mjPreContact* con, int ncon, mjtNum tolerance) {
+  const mjtNum* last_pos = con[ncon - 1].pos;
   for (int i=0; i < ncon-1; i++) {
-    if (mju_dist3(con[i].pos, con[ncon - 1].pos) <= tolerance) {
+    if (mju_dist3(con[i].pos, last_pos) <= tolerance) {
       return 0;
     }
   }
@@ -838,7 +839,7 @@ static int maxContacts(const mjModel* m, const mjCCDObj* obj1, const mjCCDObj* o
 
 
 // multi-point convex-convex collision, using libccd
-int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_Convex(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) {
   // init ccd objects
   mjCCDObj obj1, obj2;
   mjc_initCCDObj(&obj1, m, d, g1, margin);
@@ -857,7 +858,7 @@ int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN
   }
 
   // look for additional contacts
-  if (ncon == 1 && !mjDISABLED(mjDSBL_MULTICCD)  // TODO(tassa) leave as bitflag or make geom attribute (?)
+  if (ncon == 1 && !mjDISABLED(mjDSBL_MULTICCD)
       && m->geom_type[g1] != mjGEOM_ELLIPSOID && m->geom_type[g1] != mjGEOM_SPHERE
       && m->geom_type[g2] != mjGEOM_ELLIPSOID && m->geom_type[g2] != mjGEOM_SPHERE) {
     // multiCCD parameters
@@ -866,7 +867,8 @@ int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN
 
     // complete frame of initial contact
     mjtNum frame[9];
-    mji_copy9(frame, con[0].frame);
+    mji_copy3(frame, con[0].normal);
+    mju_zero(frame+3, 6);
     mju_makeFrame(frame);
 
     // tolerance for determining if newly found contacts are distinct
@@ -926,7 +928,7 @@ const int maxplanemesh = 3;
 const mjtNum tolplanemesh = 0.3;
 
 // add one plane-mesh contact
-static int addplanemesh(mjContact* con, const float vertex[3],
+static int addplanemesh(mjPreContact* con, const float vertex[3],
                         const mjtNum pos1[3], const mjtNum normal1[3],
                         const mjtNum pos2[3], const mjtNum mat2[9],
                         const mjtNum first[3], mjtNum rbound) {
@@ -945,44 +947,47 @@ static int addplanemesh(mjContact* con, const float vertex[3],
   mji_sub3(dif, pnt, pos1);
 
   // set distance
-  con->dist = mju_dot3(normal1, dif);
+  con[0].dist = mju_dot3(normal1, dif);
 
   // set position
-  mji_copy3(con->pos, pnt);
-  mji_addToScl3(con->pos, normal1, -0.5*con->dist);
+  mji_copy3(con[0].pos, pnt);
+  mji_addToScl3(con[0].pos, normal1, -0.5*con[0].dist);
 
   // set frame
-  mji_copy3(con->frame, normal1);
-  mju_zero3(con->frame+3);
+  mji_copy3(con[0].normal, normal1);
+  mji_zero3(con[0].tangent);
 
   return 1;
 }
 
 
 // plane-convex collision, using libccd
-int mjc_PlaneConvex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
-  mjGETINFO
-  mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]};
-  ccd_vec3_t dir, vec;
+int mjc_PlaneConvex(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) {
+  const mjtNum* pos1  = d->geom_xpos + 3*g1;
+  const mjtNum* mat1  = d->geom_xmat + 9*g1;
+  const mjtNum* pos2  = d->geom_xpos + 3*g2;
+  const mjtNum* mat2  = d->geom_xmat + 9*g2;
+
+  mjtNum dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]};
+  ccd_vec3_t ccd_dir, ccd_vec;
   mjCCDObj obj;
   mjc_initCCDObj(&obj, m, d, g2, 0);
   // get support point in -normal direction
-  ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]);
-  mjccd_support(&obj, &dir, &vec);
+  ccdVec3Set(&ccd_dir, -mat1[2], -mat1[5], -mat1[8]);
+  mjccd_support(&obj, &ccd_dir, &ccd_vec);
 
   // compute normal distance, return if too far
-  mji_sub3(dif, vec.v, pos1);
-  dist = mju_dot3(normal, dif);
-  if (dist > margin) {
+  mji_sub3(dif, ccd_vec.v, pos1);
+  con[0].dist = mju_dot3(normal, dif);
+  if (con[0].dist > margin) {
     return 0;
   }
 
   // fill in contact data
-  con->dist = dist;
-  mji_copy3(con->pos, vec.v);
-  mji_addToScl3(con->pos, normal, -0.5*dist);
-  mji_copy3(con->frame, normal);
-  mju_zero3(con->frame+3);
+  mji_copy3(con[0].pos, ccd_vec.v);
+  mji_addToScl3(con[0].pos, normal, -0.5*con[0].dist);
+  mji_copy3(con[0].normal, normal);
+  mji_zero3(con[0].tangent);
 
   //--------------- add all/connected vertices below margin
   float* vertdata;
@@ -1001,7 +1006,7 @@ int mjc_PlaneConvex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
 
   // express dir in geom local frame
   mjtNum locdir[3];
-  mju_mulMatTVec3(locdir, d->geom_xmat+9*g, dir.v);
+  mju_mulMatTVec3(locdir, d->geom_xmat+9*g, ccd_dir.v);
 
   // inclusion threshold along locdir, relative to geom2 center
   mji_sub3(dif, pos2, pos1);
@@ -1097,7 +1102,8 @@ static inline void addPrismVert(mjCCDObj* obj, int r, int c, int i, mjtNum dx, m
 
 
 // entry point for heightfield collisions
-int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_ConvexHField(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
+                     mjtNum margin) {
   // hfield frame
   const mjtNum* pos1 = d->geom_xpos + 3*g1;
   const mjtNum* mat1 = d->geom_xmat + 9*g1;
@@ -1116,16 +1122,16 @@ int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2
   // try early return using box-sphere test
 
   // express geom2 pos in hfield frame
-  mjtNum pos[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]};
-  mju_mulMatTVec3(pos, mat1, pos);
+  mjtNum local_pos[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]};
+  mju_mulMatTVec3(local_pos, mat1, local_pos);
 
   // sphere radius is geom2 rbound + margin
   mjtNum radius = m->geom_rbound[g2] + margin;
 
   // box-sphere test
-  if ((size0 < pos[0] - radius) || (-size0 > pos[0] + radius) ||
-      (size1 < pos[1] - radius) || (-size1 > pos[1] + radius) ||
-      (size2 < pos[2] - radius) || (-size3 > pos[2] + radius)) {
+  if ((size0 < local_pos[0] - radius) || (-size0 > local_pos[0] + radius) ||
+      (size1 < local_pos[1] - radius) || (-size1 > local_pos[1] + radius) ||
+      (size2 < local_pos[2] - radius) || (-size3 > local_pos[2] + radius)) {
     return 0;
   }
 
@@ -1142,42 +1148,42 @@ int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2
   mji_mulMatTMat3(mat, mat1, mat2);
 
   mji_copy9(obj2.mat, mat);
-  mji_copy3(obj2.pos, pos);
+  mji_copy3(obj2.pos, local_pos);
 
-  mjtNum dir[3] = {0, 0, 0}, res[3];
+  mjtNum local_dir[3] = {0, 0, 0}, res[3];
 
   // get support point in +X
-  dir[0] = 1;
-  obj2.support(res, &obj2, dir);
+  local_dir[0] = 1;
+  obj2.support(res, &obj2, local_dir);
   mjtNum xmax = res[0];
 
   // get support point in -X
-  dir[0] = -1;
-  obj2.support(res, &obj2, dir);
+  local_dir[0] = -1;
+  obj2.support(res, &obj2, local_dir);
   mjtNum xmin = res[0];
 
-  dir[0] = 0;
+  local_dir[0] = 0;
 
   // get support point in +Y
-  dir[1] = 1;
-  obj2.support(res, &obj2, dir);
+  local_dir[1] = 1;
+  obj2.support(res, &obj2, local_dir);
   mjtNum ymax = res[1];
 
   // get support point in -Y
-  dir[1] = -1;
-  obj2.support(res, &obj2, dir);
+  local_dir[1] = -1;
+  obj2.support(res, &obj2, local_dir);
   mjtNum ymin = res[1];
 
-  dir[1] = 0;
+  local_dir[1] = 0;
 
   // get support point in +Z
-  dir[2] = 1;
-  obj2.support(res, &obj2, dir);
+  local_dir[2] = 1;
+  obj2.support(res, &obj2, local_dir);
   mjtNum zmax = res[2];
 
   // get support point in -Z
-  dir[2] = -1;
-  obj2.support(res, &obj2, dir);
+  local_dir[2] = -1;
+  obj2.support(res, &obj2, local_dir);
   mjtNum zmin = res[2];
 
   // AABB box-box test
@@ -1226,10 +1232,10 @@ int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2
         // run penetration function, save contact
         if (mjc_penetration(m, d, &obj1, &obj2, con + ncon, 1, 0.0)) {
           // transform to global coordinates
-          mji_copy3(dir, con[ncon].frame);
-          mji_copy3(pos, con[ncon].pos);
-          mji_mulMatVec3(con[ncon].frame, mat1, dir);
-          mji_mulMatVec3(con[ncon].pos, mat1, pos);
+          mji_copy3(local_dir, con[ncon].normal);
+          mji_copy3(local_pos, con[ncon].pos);
+          mji_mulMatVec3(con[ncon].normal, mat1, local_dir);
+          mji_mulMatVec3(con[ncon].pos, mat1, local_pos);
           mji_addTo3(con[ncon].pos, pos1);
 
           // force out of all loops if max contacts reached
@@ -1247,7 +1253,7 @@ int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2
   if (mjDISABLED(mjDSBL_NATIVECCD)) {
     // fix contact normals
     for (int i=0; i < ncon; i++) {
-      mjc_fixNormal(m, d, con+i, g1, g2);
+      mjc_fixNormal(m, d, con + i, g1, g2);
     }
   }
 
@@ -1362,7 +1368,7 @@ static int mjc_ellipsoidOutside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum
 
 
 // fix normals if required
-void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2) {
+void mjc_fixNormal(const mjModel* m, const mjData* d, mjPreContact* con, int g1, int g2) {
   mjtNum dst1, dst2;
 
   // get geom ids and types
@@ -1389,8 +1395,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
 
   // init normals
   mjtNum normal[2][3] = {
-    {con->frame[0], con->frame[1], con->frame[2]},
-    {-con->frame[0], -con->frame[1], -con->frame[2]}
+    {con->normal[0], con->normal[1], con->normal[2]},
+    {-con->normal[0], -con->normal[1], -con->normal[2]}
   };
 
   // process geoms in type range
@@ -1402,27 +1408,27 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
       mjtNum* size = m->geom_size + 3*gid[i];
 
       // map contact point and normal to local frame
-      mjtNum dif[3], pos[3], nrm[3];
+      mjtNum dif[3], pos1[3], nrm[3];
       mju_sub3(dif, con->pos, d->geom_xpos+3*gid[i]);
-      mju_mulMatTVec3(pos, mat, dif);
+      mju_mulMatTVec3(pos1, mat, dif);
       mju_mulMatTVec3(nrm, mat, normal[i]);
 
       // process according to type
       switch (type[i]) {
       case mjGEOM_SPHERE:
-        mji_copy3(nrm, pos);
+        mji_copy3(nrm, pos1);
         processed[i] = 1;
         break;
 
       case mjGEOM_CAPSULE:
         // Z: bottom cap
-        if (pos[2] < -size[1]) {
-          nrm[2] = pos[2]+size[1];
+        if (pos1[2] < -size[1]) {
+          nrm[2] = pos1[2]+size[1];
         }
 
         // Z: top cap
-        else if (pos[2] > size[1]) {
-          nrm[2] = pos[2]-size[1];
+        else if (pos1[2] > size[1]) {
+          nrm[2] = pos1[2]-size[1];
         }
 
         // Z: cylinder
@@ -1431,8 +1437,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
         }
 
         // copy XY
-        nrm[0] = pos[0];
-        nrm[1] = pos[1];
+        nrm[0] = pos1[0];
+        nrm[1] = pos1[1];
         processed[i] = 1;
         break;
 
@@ -1443,27 +1449,27 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
         }
 
         // compute elliptic distance^2
-        dst1 = pos[0]*pos[0]/(size[0]*size[0]) +
-               pos[1]*pos[1]/(size[1]*size[1]) +
-               pos[2]*pos[2]/(size[2]*size[2]);
+        dst1 = pos1[0]*pos1[0]/(size[0]*size[0]) +
+               pos1[1]*pos1[1]/(size[1]*size[1]) +
+               pos1[2]*pos1[2]/(size[2]*size[2]);
 
         // dispatch to inside or outside solver
         if (dst1 <= 1) {
-          processed[i] = mjc_ellipsoidInside(nrm, pos, size);
+          processed[i] = mjc_ellipsoidInside(nrm, pos1, size);
         } else {
-          processed[i] = mjc_ellipsoidOutside(nrm, pos, size);
+          processed[i] = mjc_ellipsoidOutside(nrm, pos1, size);
         }
         break;
 
       case mjGEOM_CYLINDER:
         // skip if within 5% length of flat wall
-        if (mju_abs(pos[2]) > 0.95*size[1]) {
+        if (mju_abs(pos1[2]) > 0.95*size[1]) {
           break;
         }
 
         // compute distances to flat and round wall
-        dst1 = mju_abs(size[1]-mju_abs(pos[2]));
-        dst2 = mju_abs(size[0]-mju_norm(pos, 2));
+        dst1 = mju_abs(size[1]-mju_abs(pos1[2]));
+        dst2 = mju_abs(size[0]-mju_norm(pos1, 2));
 
         // require 4x closer to round than flat wall
         if (dst1 < 0.25*dst2) {
@@ -1471,8 +1477,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
         }
 
         // set normal for round wall
-        nrm[0] = pos[0];
-        nrm[1] = pos[1];
+        nrm[0] = pos1[0];
+        nrm[1] = pos1[1];
         nrm[2] = 0;
         processed[i] = 1;
         break;
@@ -1492,23 +1498,18 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
 
   // both processed: average
   if (processed[0] && processed[1]) {
-    mji_sub3(con->frame, normal[0], normal[1]);
-    mju_normalize3(con->frame);
+    mji_sub3(con->normal, normal[0], normal[1]);
+    mju_normalize3(con->normal);
   }
 
   // first processed: copy
   else if (processed[0]) {
-    mji_copy3(con->frame, normal[0]);
+    mji_copy3(con->normal, normal[0]);
   }
 
   // second processed: copy reverse
   else if (processed[1]) {
-    mji_scl3(con->frame, normal[1], -1);
-  }
-
-  // clear second frame axis if processed, just in case
-  if (processed[0] || processed[1]) {
-    mju_zero3(con->frame+3);
+    mji_scl3(con->normal, normal[1], -1);
   }
 }
 
@@ -1525,7 +1526,8 @@ int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1,
   mjc_setCCDObjFlex(&obj2, f2, e2, -1);
 
   // find contacts
-  int ncon = mjc_penetration(m, d, &obj1, &obj2, con, 1, margin);
+  mjPreContact precon[8];
+  int ncon = mjc_penetration(m, d, &obj1, &obj2, precon, 1, margin);
 
   // fix normals for 2D flex
   if (ncon && !mjDISABLED(mjDSBL_NATIVECCD)) {
@@ -1536,11 +1538,19 @@ int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1,
 
     if (isflex2d) {
       for (int i = 0; i < ncon; i++) {
-        mjc_fixNormal(m, d, con + i, g1, -1);
+        mjc_fixNormal(m, d, precon + i, g1, -1);
       }
     }
   }
 
+  // copy to con
+  for (int i = 0; i < ncon; i++) {
+    con[i].dist = precon[i].dist;
+    mji_copy3(con[i].pos, precon[i].pos);
+    mji_copy3(con[i].frame, precon[i].normal);
+    mju_zero3(con[i].frame + 3);
+  }
+
   return ncon;
 }
 
@@ -1659,14 +1669,14 @@ int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, in
           }
 
           // run ccd, save contact
-          if (mjc_penetration(m, d, &obj1, &obj2, con + cnt, 1, 0.0)) {
+          mjContact* ccon = con + cnt;
+          mjPreContact precon;
+          if (mjc_penetration(m, d, &obj1, &obj2, &precon, 1, 0.0)) {
             // transform to global coordinates
-            mjtNum dir[3], pos[3];
-            mji_copy3(dir, con[cnt].frame);
-            mji_copy3(pos, con[cnt].pos);
-            mji_mulMatVec3(con[cnt].frame, hmat, dir);
-            mji_mulMatVec3(con[cnt].pos, hmat, pos);
-            mji_addTo3(con[cnt].pos, hpos);
+            mji_zero3(ccon->frame + 3);
+            mji_mulMatVec3(ccon->frame, hmat, precon.normal);
+            mji_mulMatVec3(ccon->pos, hmat, precon.pos);
+            mji_addTo3(ccon->pos, hpos);
 
             // count, stop if max number reached
             cnt++;

src/engine/engine_collision_convex.h

@@ -109,9 +109,9 @@ void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]);
 void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]);
 
 // pairwise geom collision functions using ccd
-int mjc_PlaneConvex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
-int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
-MJAPI int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
+int mjc_PlaneConvex(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin);
+int mjc_ConvexHField(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin);
+MJAPI int mjc_Convex(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin);
 
 // geom-elem or elem-elem or vert-elem collision function using ccd
 int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1, int e1, int v1,
@@ -122,7 +122,7 @@ int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, in
                    mjtNum margin);
 
 // fix contact frame normal
-void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2);
+void mjc_fixNormal(const mjModel* m, const mjData* d, mjPreContact* con, int g1, int g2);
 
 #ifdef __cplusplus
 }

src/engine/engine_collision_driver.c

@@ -1745,20 +1745,11 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) {
     return;
   }
 
-  // allocate mjContact[mjMAXCONPAIR] on the arena
-  mjContact* con =
-    (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact));
-  if (!con) {
-    mj_warning(d, mjWARN_CONTACTFULL, d->ncon);
-    return;
-  }
+
 
   // call collision detector to generate contacts
-  num = collisionFunc(m, d, con, g1, g2, margin + gap);
-
-  // check contacts
-  if (!num) {
-    resetArena(d);
+  mjPreContact precon[mjMAXCONPAIR];
+  if (!(num = collisionFunc(m, d, precon, g1, g2, margin + gap))) {
     return;
   }
 
@@ -1767,6 +1758,12 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) {
     mjERROR("too many contacts returned by collision function");
   }
 
+  mjContact* con = (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * num, _Alignof(mjContact));
+  if (!con) {
+    mj_warning(d, mjWARN_CONTACTFULL, d->ncon);
+    return;
+  }
+
   // set condim, solref, solimp, friction: dynamic
   if (ipair < 0) {
     mj_contactParam(m, &condim, solref, solimp, friction, g1, g2, -1, -1);
@@ -1787,7 +1784,12 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) {
 
   // add contacts returned by collision detector
   for (int i=0; i < num; i++) {
-    // set contact ids
+    // set contact parameters
+    con[i].dist = precon[i].dist;
+    mji_copy3(con[i].pos, precon[i].pos);
+    mji_copy3(con[i].frame + 0, precon[i].normal);
+    mji_copy3(con[i].frame + 3, precon[i].tangent);
+
     con[i].geom[0] = g1;
     con[i].geom[1] = g2;
     con[i].flex[0] = -1;
@@ -2109,23 +2111,32 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
                    pos, mat, size);
 
     // call raw primitive for corresponding geom type
-    if (type == mjGEOM_SPHERE) {
-      num = mjraw_SphereCapsule(con, margin + gap,
-                                d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g,
-                                pos, mat, size);
+    mjPreContact precon[2];
+    switch (type) {
+      case mjGEOM_SPHERE:
+        num = mjraw_SphereCapsule(precon, margin + gap, d->geom_xpos+3*g, d->geom_xmat+9*g,
+                                  m->geom_size+3*g, pos, mat, size);
+        break;
+      case mjGEOM_CAPSULE:
+        num = mjraw_CapsuleCapsule(precon, margin + gap, d->geom_xpos+3*g, d->geom_xmat+9*g,
+                                   m->geom_size+3*g, pos, mat, size);
+        break;
+      case mjGEOM_BOX:
+        num = mjraw_CapsuleBox(precon, margin + gap, pos, mat, size, d->geom_xpos+3*g,
+                               d->geom_xmat+9*g, m->geom_size+3*g);
+        break;
+      default:
+        num = 0;
     }
-    else if (type == mjGEOM_CAPSULE) {
-      num = mjraw_CapsuleCapsule(con, margin + gap,
-                                 d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g,
-                                 pos, mat, size);
-    }
-    else {
-      num = mjraw_CapsuleBox(con, margin + gap,
-                             pos, mat, size,
-                             d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g);
+
+    for (int i=0; i < num; i++) {
+      con[i].dist = precon[i].dist;
+      mju_copy3(con[i].pos, precon[i].pos);
+      mju_copy3(con[i].frame + 0, precon[i].normal);
+      mju_copy3(con[i].frame + 3, precon[i].tangent);
 
       // reverse contact normals, since box geom is second
-      for (int i=0; i < num; i++) {
+      if (type == mjGEOM_BOX) {
         mju_scl3(con[i].frame, con[i].frame, -1);
       }
     }
@@ -2256,7 +2267,14 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2
                    pos2, mat2, size2);
 
     // raw primitive
-    num = mjraw_CapsuleCapsule(con, margin + gap, pos1, mat1, size1, pos2, mat2, size2);
+    mjPreContact precon[mjMAXCONPAIR];
+    num = mjraw_CapsuleCapsule(precon, margin + gap, pos1, mat1, size1, pos2, mat2, size2);
+    for (int i=0; i < num; i++) {
+      con[i].dist = precon[i].dist;
+      mju_copy3(con[i].pos, precon[i].pos);
+      mju_copy3(con[i].frame + 0, precon[i].normal);
+      mju_copy3(con[i].frame + 3, precon[i].tangent);
+    }
   }
 
   // general convex collision
@@ -2332,7 +2350,12 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) {
     mjtNum pos[3], mat[9], size[2];
     mjtNum I[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
     mj_makeCapsule(m, d, f, edata, pos, mat, size);
-    num = mjraw_SphereCapsule(con, 0, vert, I, &radius, pos, mat, size);
+    mjPreContact precon;
+    num = mjraw_SphereCapsule(&precon, 0, vert, I, &radius, pos, mat, size);
+    con->dist = precon.dist;
+    mju_copy3(con->pos, precon.pos);
+    mju_copy3(con->frame + 0, precon.normal);
+    mju_copy3(con->frame + 3, precon.tangent);
   }
 
   // sphere : triangle

src/engine/engine_collision_primitive.c

@@ -25,70 +25,87 @@
 //--------------------------- plane collisions -----------------------------------------------------
 
 // raw plane : sphere
-static int mjraw_PlaneSphere(mjContact* con, mjtNum margin,
+static int mjraw_PlaneSphere(mjPreContact* con, mjtNum margin,
                              const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
                              const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
   // set normal
-  con[0].frame[0] = mat1[2];
-  con[0].frame[1] = mat1[5];
-  con[0].frame[2] = mat1[8];
+  con[0].normal[0] = mat1[2];
+  con[0].normal[1] = mat1[5];
+  con[0].normal[2] = mat1[8];
 
   // compute distance, return if too large
   mjtNum tmp[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]};
-  mjtNum cdist = mju_dot3(tmp, con[0].frame);
+  mjtNum cdist = mju_dot3(tmp, con[0].normal);
   if (cdist > margin + size2[0]) {
     return 0;
   }
 
   // depth and position
   con[0].dist = cdist - size2[0];
-  mji_scl3(tmp, con[0].frame, -con[0].dist / 2 - size2[0]);
+  mji_scl3(tmp, con[0].normal, -con[0].dist / 2 - size2[0]);
   mji_add3(con[0].pos, pos2, tmp);
-
-  mju_zero3(con[0].frame+3);
+  mji_zero3(con[0].tangent);
   return 1;
 }
 
 
 // plane : sphere
-int mjc_PlaneSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
-  mjGETINFO
+int mjc_PlaneSphere(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
+                    mjtNum margin) {
+  const mjtNum* pos1 = d->geom_xpos + 3*g1;
+  const mjtNum* pos2 = d->geom_xpos + 3*g2;
+  const mjtNum* mat1 = d->geom_xmat + 9*g1;
+  const mjtNum* mat2 = d->geom_xmat + 9*g2;
+  const mjtNum* size1 = m->geom_size + 3*g1;
+  const mjtNum* size2 = m->geom_size + 3*g2;
   return mjraw_PlaneSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
 }
 
 
 // plane : capsule
-int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
-  mjGETINFO
+int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
+                     mjtNum margin) {
+  const mjtNum* pos1 = d->geom_xpos + 3*g1;
+  const mjtNum* pos2 = d->geom_xpos + 3*g2;
+  const mjtNum* mat1 = d->geom_xmat + 9*g1;
+  const mjtNum* mat2 = d->geom_xmat + 9*g2;
+  const mjtNum* size1 = m->geom_size + 3*g1;
+  const mjtNum* size2 = m->geom_size + 3*g2;
 
   // get capsule axis, segment = scaled axis
   mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]};
   mjtNum segment[3] = {size2[1]*axis[0], size2[1]*axis[1], size2[1]*axis[2]};
 
   // get point 1, do sphere-plane test
-  mjtNum pos[3];
-  mju_add3(pos, pos2, segment);
-  int n1 = mjraw_PlaneSphere(con, margin, pos1, mat1, size1, pos, mat2, size2);
+  mjtNum endpoint[3];
+  mju_add3(endpoint, pos2, segment);
+  int n1 = mjraw_PlaneSphere(con, margin, pos1, mat1, size1, endpoint, mat2, size2);
 
   // get point 2, do sphere-plane test
-  mju_sub3(pos, pos2, segment);
-  int n2 = mjraw_PlaneSphere(con+n1, margin, pos1, mat1, size1, pos, mat2, size2);
+  mju_sub3(endpoint, pos2, segment);
+  int n2 = mjraw_PlaneSphere(con + n1, margin, pos1, mat1, size1, endpoint, mat2, size2);
 
   // align contact frames with capsule axis
   if (n1) {
-    mji_copy3(con->frame + 3, axis);
+    mji_copy3(con[0].tangent, axis);
   }
   if (n2) {
-    mji_copy3((con + n1)->frame + 3, axis);
+    mji_copy3(con[n1].tangent, axis);
   }
 
-  return n1+n2;
+  return n1 + n2;
 }
 
 
 // plane : cylinder
-int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
-  mjGETINFO
+int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
+                      mjtNum margin) {
+  const mjtNum* pos1 = d->geom_xpos + 3*g1;
+  const mjtNum* pos2 = d->geom_xpos + 3*g2;
+  const mjtNum* mat1 = d->geom_xmat + 9*g1;
+  const mjtNum* mat2 = d->geom_xmat + 9*g2;
+  const mjtNum* size2 = m->geom_size + 3*g2;
+
   mjtNum normal[3] = {mat1[2], mat1[5], mat1[8]};
   mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]};
 
@@ -137,8 +154,8 @@ int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g
     mji_add3(con[cnt].pos, pos2, vec);
     mji_addTo3(con[cnt].pos, axis);
     mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5);
-    mji_copy3(con[cnt].frame, normal);
-    mju_zero3(con[cnt].frame+3);
+    mji_copy3(con[cnt].normal, normal);
+    mji_zero3(con[cnt].tangent);
     cnt++;
   } else {
     return 0;  // nearest point is above margin: no contacts
@@ -150,8 +167,8 @@ int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g
     mji_add3(con[cnt].pos, pos2, vec);
     mji_subFrom3(con[cnt].pos, axis);
     mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5);
-    mji_copy3(con[cnt].frame, normal);
-    mju_zero3(con[cnt].frame+3);
+    mji_copy3(con[cnt].normal, normal);
+    mji_zero3(con[cnt].tangent);
     cnt++;
   }
 
@@ -170,8 +187,8 @@ int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g
     mji_addTo3(con[cnt].pos, axis);
     mji_addToScl3(con[cnt].pos, vec, -0.5);
     mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5);
-    mji_copy3(con[cnt].frame, normal);
-    mju_zero3(con[cnt].frame+3);
+    mji_copy3(con[cnt].normal, normal);
+    mji_zero3(con[cnt].tangent);
     cnt++;
 
     // add point B
@@ -180,8 +197,8 @@ int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g
     mji_addTo3(con[cnt].pos, axis);
     mji_addToScl3(con[cnt].pos, vec, -0.5);
     mji_addToScl3(con[cnt].pos, normal, -con[cnt].dist * 0.5);
-    mji_copy3(con[cnt].frame, normal);
-    mju_zero3(con[cnt].frame+3);
+    mji_copy3(con[cnt].normal, normal);
+    mji_zero3(con[cnt].tangent);
     cnt++;
   }
 
@@ -190,8 +207,12 @@ int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g
 
 
 // plane : box
-int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
-  mjGETINFO
+int mjc_PlaneBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) {
+  const mjtNum* pos1 = d->geom_xpos + 3*g1;
+  const mjtNum* pos2 = d->geom_xpos + 3*g2;
+  const mjtNum* mat1 = d->geom_xmat + 9*g1;
+  const mjtNum* mat2 = d->geom_xmat + 9*g2;
+  const mjtNum* size2 = m->geom_size + 3*g2;
 
   // get normal, difference between centers, normal distance
   mjtNum norm[3] = {mat1[2], mat1[5], mat1[8]};
@@ -219,11 +240,11 @@ int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mj
 
     // construct contact
     con[cnt].dist = dist + ldist;
-    mji_copy3(con[cnt].frame, norm);
-    mju_zero3(con[cnt].frame+3);
+    mji_copy3(con[cnt].normal, norm);
     mji_addTo3(corner, pos2);
     mji_scl3(vec, norm, -con[cnt].dist / 2);
     mji_add3(con[cnt].pos, corner, vec);
+    mji_zero3(con[cnt].tangent);
 
     // count; max is 4
     if (++cnt >= 4) {
@@ -238,7 +259,7 @@ int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mj
 //--------------------------- sphere and capsule collisions ----------------------------------------
 
 // sphere : sphere (actual implementation, can be called with modified parameters)
-static int mjraw_SphereSphere(mjContact* con, mjtNum margin,
+static int mjraw_SphereSphere(mjPreContact* con, mjtNum margin,
                               const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
                               const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
   // check bounding spheres (this is called from other functions)
@@ -251,36 +272,45 @@ static int mjraw_SphereSphere(mjContact* con, mjtNum margin,
 
   // depth and normal
   con[0].dist = mju_sqrt(cdist_sqr) - size1[0] - size2[0];
-  mji_sub3(con[0].frame, pos2, pos1);
-  mjtNum len = mju_normalize3(con[0].frame);
+  mji_sub3(con[0].normal, pos2, pos1);
+  mjtNum len = mju_normalize3(con[0].normal);
 
   // if centers are the same, norm = cross-product of z axes
   //  if z axes are parallel, norm = [1;0;0]
   if (len < mjMINVAL) {
     mjtNum axis1[3] = {mat1[2], mat1[5], mat1[8]};
     mjtNum axis2[3] = {mat2[2], mat2[5], mat2[8]};
-    mji_cross(con[0].frame, axis1, axis2);
-    mju_normalize3(con[0].frame);
+    mji_cross(con[0].normal, axis1, axis2);
+    mju_normalize3(con[0].normal);
   }
 
   // position
-  mji_scl3(con[0].pos, con[0].frame, size1[0] + con[0].dist / 2);
+  mji_scl3(con[0].pos, con[0].normal, size1[0] + con[0].dist / 2);
   mji_addTo3(con[0].pos, pos1);
 
-  mju_zero3(con[0].frame+3);
+  // axis
+  mji_zero3(con[0].tangent);
+
   return 1;
 }
 
 
 // sphere : sphere
-int mjc_SphereSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
-  mjGETINFO
+int mjc_SphereSphere(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
+                     mjtNum margin) {
+  const mjtNum* pos1  = d->geom_xpos + 3*g1;
+  const mjtNum* mat1  = d->geom_xmat + 9*g1;
+  const mjtNum* size1 = m->geom_size + 3*g1;
+  const mjtNum* pos2  = d->geom_xpos + 3*g2;
+  const mjtNum* mat2  = d->geom_xmat + 9*g2;
+  const mjtNum* size2 = m->geom_size + 3*g2;
   return mjraw_SphereSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
 }
 
 
+
 // raw sphere : capsule
-int mjraw_SphereCapsule(mjContact* con, mjtNum margin,
+int mjraw_SphereCapsule(mjPreContact* con, mjtNum margin,
                         const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
                         const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
   // get capsule length and axis
@@ -299,16 +329,27 @@ int mjraw_SphereCapsule(mjContact* con, mjtNum margin,
 
 
 // sphere : capsule
-int mjc_SphereCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
-  mjGETINFO
+int mjc_SphereCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
+                      mjtNum margin) {
+  const mjtNum* pos1 = d->geom_xpos + 3*g1;
+  const mjtNum* pos2 = d->geom_xpos + 3*g2;
+  const mjtNum* mat1 = d->geom_xmat + 9*g1;
+  const mjtNum* mat2 = d->geom_xmat + 9*g2;
+  const mjtNum* size1 = m->geom_size + 3*g1;
+  const mjtNum* size2 = m->geom_size + 3*g2;
   return mjraw_SphereCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
 }
 
 
 // sphere : cylinder
-int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+int mjc_SphereCylinder(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
                        mjtNum margin) {
-  mjGETINFO
+  const mjtNum* pos1 = d->geom_xpos + 3*g1;
+  const mjtNum* pos2 = d->geom_xpos + 3*g2;
+  const mjtNum* mat1 = d->geom_xmat + 9*g1;
+  const mjtNum* mat2 = d->geom_xmat + 9*g2;
+  const mjtNum* size1 = m->geom_size + 3*g1;
+  const mjtNum* size2 = m->geom_size + 3*g2;
 
   // get cylinder sizes and axis
   mjtNum radius = size2[0];
@@ -359,9 +400,10 @@ int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int
       mat_cap = flipmat;
     }
     int ncon = mjraw_PlaneSphere(con, margin, pos_cap, mat_cap, size2, pos1, mat1, size1);
+
     if (ncon) {
-      // flip frame normal (because mjGEOM_PLANE < mjGEOM_SPHERE < mjGEOM_CYLINDER)
-      mju_scl3(con->frame, con->frame, -1);
+      // flip direction normal (because mjGEOM_PLANE < mjGEOM_SPHERE < mjGEOM_CYLINDER)
+      mju_scl3(con[0].normal, con[0].normal, -1);
     }
     return ncon;
   }
@@ -380,7 +422,7 @@ int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int
 
 
 // raw capsule : capsule
-int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
+int mjraw_CapsuleCapsule(mjPreContact* con, mjtNum margin,
                          const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
                          const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
   // get capsule axes (scaled) and center difference
@@ -444,7 +486,7 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
     x2 = mju_clip((v + mb) / mc, -1, 1);
     mji_scl3(vec2, axis2, x2);
     mji_addTo3(vec2, pos2);
-    int n2 = mjraw_SphereSphere(con+n1, margin, vec1, mat1, size1, vec2, mat2, size2);
+    int n2 = mjraw_SphereSphere(con + n1, margin, vec1, mat1, size1, vec2, mat2, size2);
 
     // return if two contacts already found
     if (n1+n2 >= 2) {
@@ -456,7 +498,7 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
     mjtNum x1 = mju_clip((u - mb) / ma, -1, 1);
     mji_scl3(vec1, axis1, x1);
     mji_addTo3(vec1, pos1);
-    int n3 = mjraw_SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2);
+    int n3 = mjraw_SphereSphere(con + n1 + n2, margin, vec1, mat1, size1, vec2, mat2, size2);
 
     // return if two contacts already found
     if (n1+n2+n3 >= 2) {
@@ -468,7 +510,7 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
     x1 = mju_clip((u + mb) / ma, -1, 1);
     mji_scl3(vec1, axis1, x1);
     mji_addTo3(vec1, pos1);
-    int n4 = mjraw_SphereSphere(con+n1+n2+n3, margin, vec1, mat1, size1, vec2, mat2, size2);
+    int n4 = mjraw_SphereSphere(con + n1 + n2 + n3, margin, vec1, mat1, size1, vec2, mat2, size2);
 
     return n1+n2+n3+n4;
   }
@@ -476,9 +518,14 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
 
 
 // capsule : capsule
-int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
-                       mjtNum margin) {
-  mjGETINFO
+int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
+                        mjtNum margin) {
+  const mjtNum* pos1 = d->geom_xpos + 3*g1;
+  const mjtNum* pos2 = d->geom_xpos + 3*g2;
+  const mjtNum* mat1 = d->geom_xmat + 9*g1;
+  const mjtNum* mat2 = d->geom_xmat + 9*g2;
+  const mjtNum* size1 = m->geom_size + 3*g1;
+  const mjtNum* size2 = m->geom_size + 3*g2;
   return mjraw_CapsuleCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
 }
 
@@ -601,6 +648,7 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin,
   return 1;
 }
 
+
 // box : triangle with radius
 int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos,
                       const mjtNum* mat, const mjtNum* size, const mjtNum* t1,
@@ -693,6 +741,7 @@ int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos,
   return cnt;
 }
 
+
 // capsule : triangle with radius
 int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos,
                           const mjtNum* mat, const mjtNum* size,

src/engine/engine_collision_primitive.h

@@ -18,68 +18,57 @@
 #include <mujoco/mjdata.h>
 #include <mujoco/mjexport.h>
 #include <mujoco/mjmodel.h>
-
-// define and extract geom info
-#define mjGETINFO \
-    const mjtNum* pos1  = d->geom_xpos + 3*g1; \
-    const mjtNum* mat1  = d->geom_xmat + 9*g1; \
-    const mjtNum* size1 = m->geom_size + 3*g1; \
-    const mjtNum* pos2  = d->geom_xpos + 3*g2; \
-    const mjtNum* mat2  = d->geom_xmat + 9*g2; \
-    const mjtNum* size2 = m->geom_size + 3*g2; \
-    (void) size1; (void) size2;  // size1 and size2 are sometimes unused
+#include <mujoco/mjtnum.h>
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
 // raw collision functions (called by mjc_XXX)
-int mjraw_SphereCapsule (mjContact* con, mjtNum margin,
-                         const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
-                         const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2);
-int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
-                         const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
-                         const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2);
-int mjraw_CapsuleBox    (mjContact* con, mjtNum margin,
-                         const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
-                         const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2);
-int mjraw_SphereTriangle(mjContact* con, mjtNum margin,
-                         const mjtNum* s, mjtNum rs,
+int mjraw_SphereCapsule(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1,
+                        const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2,
+                        const mjtNum* size2);
+int mjraw_CapsuleCapsule(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1,
+                         const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2,
+                         const mjtNum* size2);
+int mjraw_CapsuleBox(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1,
+                     const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2,
+                     const mjtNum* size2);
+int mjraw_SphereTriangle(mjContact* con, mjtNum margin, const mjtNum* s, mjtNum rs,
                          const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt);
-int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos,
-                      const mjtNum* mat, const mjtNum* size, const mjtNum* t1,
-                      const mjtNum* t2, const mjtNum* t3, mjtNum rt);
-int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos,
-                          const mjtNum* mat, const mjtNum* size,
-                          const mjtNum* t1, const mjtNum* t2, const mjtNum* t3,
+int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, const mjtNum* mat,
+                      const mjtNum* size, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3,
+                      mjtNum rt);
+int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, const mjtNum* mat,
+                          const mjtNum* size, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3,
                           mjtNum rt);
 
 // plane collisions
-MJAPI int mjc_PlaneSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+MJAPI int mjc_PlaneSphere(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
                           mjtNum margin);
-MJAPI int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+MJAPI int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
                            mjtNum margin);
-MJAPI int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+MJAPI int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
                             mjtNum margin);
-MJAPI int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+MJAPI int mjc_PlaneBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
                        mjtNum margin);
 
 // sphere and capsule collisions
-MJAPI int mjc_SphereSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
-                           mjtNum margin);
-MJAPI int mjc_SphereCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+MJAPI int mjc_SphereSphere(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
                             mjtNum margin);
-MJAPI int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+MJAPI int mjc_SphereCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
+                            mjtNum margin);
+MJAPI int mjc_SphereCylinder(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
                              mjtNum margin);
-MJAPI int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+MJAPI int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
                              mjtNum margin);
 
 // box collisions: from engine_collision_box.c
-MJAPI int mjc_CapsuleBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+MJAPI int mjc_CapsuleBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
                          mjtNum margin);
-MJAPI int mjc_SphereBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+MJAPI int mjc_SphereBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
                         mjtNum margin);
-MJAPI int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+MJAPI int mjc_BoxBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2,
                      mjtNum margin);
 
 #ifdef __cplusplus

src/engine/engine_collision_sdf.c

@@ -21,7 +21,6 @@
 #include <mujoco/mjmodel.h>
 #include <mujoco/mjsan.h>  // IWYU pragma: keep
 #include <mujoco/mjtnum.h>
-#include "engine/engine_collision_primitive.h"
 #include "engine/engine_plugin.h"
 #include "engine/engine_util_blas.h"
 #include "engine/engine_util_errmem.h"
@@ -543,10 +542,10 @@ static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) {
 // adds candidate point to result
 // flipNormal: 0 = normal points INTO SDF (for mesh-SDF where SDF is g2)
 //             1 = normal points OUT of SDF (for flex-SDF where SDF is g1)
-static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3],
-                      const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist,
-                      int cnt, const mjModel* m, const mjSDF* s, const mjData* d,
-                      int flipNormal) {
+static int addPreContact(mjtNum* points, mjPreContact* con, const mjtNum x[3],
+                         const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist,
+                         int cnt, const mjModel* m, const mjSDF* s, const mjData* d,
+                         int flipNormal) {
   // check if there is a collision
   if (dist > 0 || isknown(points, x, cnt)) {
     return cnt;
@@ -570,18 +569,37 @@ static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3],
   }
 
   // construct contact
-  con[cnt].dist = dist;
-  mju_rotVecQuat(con[cnt].frame, norm, quat2);
-  mju_zero3(con[cnt].frame+3);
-  mju_makeFrame(con[cnt].frame);
-  mju_scl3(vec, con[cnt].frame, -con[cnt].dist/2);
-  mju_rotVecQuat(con[cnt].pos, x, quat2);
-  mju_addTo3(con[cnt].pos, pos2);
-  mju_addTo3(con[cnt].pos, vec);
+  con->dist = dist;
+  mju_rotVecQuat(con->normal, norm, quat2);
+  mju_scl3(vec, con->normal, - 0.5 * dist);
+  mju_rotVecQuat(con->pos, x, quat2);
+  mju_zero3(con->tangent);
+  mju_addTo3(con->pos, pos2);
+  mju_addTo3(con->pos, vec);
 
   return cnt+1;
 }
 
+
+// adds candidate point to result
+// flipNormal: 0 = normal points INTO SDF (for mesh-SDF where SDF is g2)
+//             1 = normal points OUT of SDF (for flex-SDF where SDF is g1)
+static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3],
+                      const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist,
+                      int cnt, const mjModel* m, const mjSDF* s, const mjData* d,
+                      int flipNormal) {
+  mjPreContact precon;
+  int ncon = addPreContact(points, &precon, x, pos2, quat2, dist, cnt, m, s, d,
+                           flipNormal);
+  if (ncon > cnt) {
+    con[cnt].dist = precon.dist;
+    mju_copy3(con[cnt].pos, precon.pos);
+    mju_copy3(con[cnt].frame, precon.normal);
+    mju_zero3(con[cnt].frame + 3);
+  }
+  return ncon;
+}
+
 // finds minimum of Frank-Wolfe objective
 static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners,
                              const mjModel* m, const mjSDF* sdf, const mjData* d) {
@@ -952,18 +970,21 @@ static int meshFaceCallback(int face_id, int node, void* ctx) {
 //------------------------------ collision functions -----------------------------------------------
 
 // collision between a height field and a signed distance field
-int mjc_HFieldSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_HFieldSDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) {
   mju_warning("HField vs SDF collision not yet supported!");
   return 0;
 }
 
 
 // collision between a mesh and a signed distance field
-int mjc_MeshSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
-  mjGETINFO;
+int mjc_MeshSDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) {
+  const mjtNum* pos1 = d->geom_xpos + 3*g1;
+  const mjtNum* mat1 = d->geom_xmat + 9*g1;
+  const mjtNum* pos2 = d->geom_xpos + 3*g2;
+  const mjtNum* mat2 = d->geom_xmat + 9*g2;
 
   mjtNum offset[3], rotation[9];
-  mjtNum points[3*mjMAXCONPAIR], dist[mjMAXCONPAIR], candidate[3*mjMAXCONPAIR];
+  mjtNum points[3*mjMAXCONPAIR], dist2[mjMAXCONPAIR], candidate[3*mjMAXCONPAIR];
   int cnt = 0, ncandidate = 0;
 
   // get sdf plugin
@@ -996,7 +1017,7 @@ int mjc_MeshSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjt
   ctx.faceadr = m->mesh_faceadr[m->geom_dataid[g1]];
   ctx.nstartpts = mju_max(1, m->opt.sdf_initpoints);
   ctx.candidate = candidate;
-  ctx.dist = dist;
+  ctx.dist = dist2;
   ctx.ncandidate = &ncandidate;
 
   // BVH traversal for mesh faces
@@ -1014,34 +1035,37 @@ int mjc_MeshSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjt
   // if few candidates, add them all directly
   if (ncandidate <= mjMAXCONPAIR) {
     for (int i = 0; i < ncandidate; i++) {
-      cnt = addContact(points, con, candidate + 3*i, pos2, sdf_quat,
-                       dist[i], cnt, m, &sdf, d, 0);
+      cnt = addPreContact(points, con + cnt, candidate + 3*i, pos2, sdf_quat,
+                          dist2[i], cnt, m, &sdf, d, 0);
     }
     return cnt;
   }
 
   // farthest point sampling (FPS) for spatial coverage
   int selected_indices[mjMAXCONPAIR];
-  int nselected = selectFPS(candidate, dist, ncandidate, selected_indices, mjMAXCONPAIR);
+  int nselected = selectFPS(candidate, dist2, ncandidate, selected_indices, mjMAXCONPAIR);
 
   // add selected contacts
   for (int i = 0; i < nselected; i++) {
     int idx = selected_indices[i];
-    cnt = addContact(points, con, candidate + 3*idx, pos2, sdf_quat,
-                     dist[idx], cnt, m, &sdf, d, 0);
+    cnt = addPreContact(points, con + cnt, candidate + 3*idx, pos2, sdf_quat,
+                        dist2[idx], cnt, m, &sdf, d, 0);
   }
 
   return cnt;
 }
 
 // collision between two SDFs
-int mjc_SDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
-  mjGETINFO;
-  size1 = m->geom_aabb + 6*g1;
-  size2 = m->geom_aabb + 6*g2;
+int mjc_SDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) {
+  const mjtNum* pos1  = d->geom_xpos + 3*g1;
+  const mjtNum* mat1  = d->geom_xmat + 9*g1;
+  const mjtNum* pos2  = d->geom_xpos + 3*g2;
+  const mjtNum* mat2  = d->geom_xmat + 9*g2;
+  const mjtNum* size1 = m->geom_aabb + 6*g1;
+  const mjtNum* size2 = m->geom_aabb + 6*g2;
 
   int cnt = 0;
-  mjtNum x[3], y[3], dist, vec1[3], vec2[3];
+  mjtNum x[3], y[3], dist2, vec1[3], vec2[3];
   mjtNum aabb1[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
   mjtNum aabb2[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
   mjtNum aabb[6]  = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
@@ -1153,15 +1177,15 @@ int mjc_SDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum
 
     // gradient descent - we use a special function of the two SDF as objective
     sdf.type = mjSDFTYPE_COLLISION;
-    dist = stepGradient(x, m, &sdf, d, m->opt.sdf_iterations);
+    dist2 = stepGradient(x, m, &sdf, d, m->opt.sdf_iterations);
 
     // inexact SDFs can yield spurious collisions, filter them by projecting on the midsurface
     sdf.type = mjSDFTYPE_INTERSECTION;
-    dist = stepGradient(x, m, &sdf, d, 1);
+    dist2 = stepGradient(x, m, &sdf, d, 1);
 
     // contact point and normal - we use the midsurface where SDF1=SDF2 as zero level set
     sdf.type = mjSDFTYPE_MIDSURFACE;
-    cnt = addContact(contacts, con, x, pos2, quat2, dist, cnt, m, &sdf, d, 0);
+    cnt = addPreContact(contacts, con + cnt, x, pos2, quat2, dist2, cnt, m, &sdf, d, 0);
 
     // SHOULD NOT OCCUR
     if (cnt > mjMAXCONPAIR) {

src/engine/engine_collision_sdf.h

@@ -36,13 +36,13 @@ MJAPI void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s, mjtNu
                         const mjtNum x[3]);
 
 // collision between a height field and a signed distance field
-int mjc_HFieldSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
+int mjc_HFieldSDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin);
 
 // collision between a mesh and a signed distance field
-int mjc_MeshSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
+int mjc_MeshSDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin);
 
 // collision between two signed distance fields
-int mjc_SDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
+int mjc_SDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin);
 
 // collision between entire flex and SDF geom (batched processing)
 int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con,

src/engine/engine_support.c

@@ -553,7 +553,7 @@ static mjtNum mj_geomDistanceCCD(const mjModel* m, mjData* d, int g1, int g2,
 // returns the smallest distance between two geoms
 mjtNum mj_geomDistance(const mjModel* m, mjData* d, int geom1, int geom2, mjtNum distmax,
                        mjtNum fromto[6]) {
-  mjContact con[mjMAXCONPAIR];
+  mjPreContact con[mjMAXCONPAIR];
   mjtNum dist = distmax;
   if (fromto) mju_zero(fromto, 6);
 
@@ -594,8 +594,8 @@ mjtNum mj_geomDistance(const mjModel* m, mjData* d, int geom1, int geom2, mjtNum
   // write fromto if given and a collision has been found
   if (fromto && smallest >= 0) {
     mjtNum sign = flip ? -1 : 1;
-    mju_addScl3(fromto+0, con[smallest].pos, con[smallest].frame, -0.5*sign*dist);
-    mju_addScl3(fromto+3, con[smallest].pos, con[smallest].frame, 0.5*sign*dist);
+    mju_addScl3(fromto+0, con[smallest].pos, con[smallest].normal, -0.5*sign*dist);
+    mju_addScl3(fromto+3, con[smallest].pos, con[smallest].normal, 0.5*sign*dist);
   }
 
   return dist;

src/engine/engine_util_spatial.c

@@ -504,7 +504,7 @@ void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_force,
 }
 
 
-// make 3D frame given X axis (normal) and possibly Y axis (tangent 1)
+// make 3D frame given x-axis (normal) and possibly y-axis (tangent 1)
 void mju_makeFrame(mjtNum frame[9]) {
   mjtNum tmp[3];