Specialized box and capsule collision primitives for stable 2D flex interactions.

PiperOrigin-RevId: 864500749
Change-Id: I52aed6e29569477a8a0b6b5eb55ad483f1077c0e

src/engine/engine_collision_convex.c

@@ -1477,13 +1477,15 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
   int gid[2] = {g1, g2};
   mjtGeom type[2];
   for (int i=0; i < 2; i++) {
-    type[i] = m->geom_type[gid[i]];
+    if (gid[i] < 0) {
+      type[i] = mjGEOM_NONE;
+    } else {
+      type[i] = m->geom_type[gid[i]];
+    }
 
     // set to mjGEOM_NONE if type cannot be processed
-    if (type[i] != mjGEOM_SPHERE    &&
-        type[i] != mjGEOM_CAPSULE   &&
-        type[i] != mjGEOM_ELLIPSOID &&
-        type[i] != mjGEOM_CYLINDER) {
+    if (type[i] != mjGEOM_SPHERE && type[i] != mjGEOM_CAPSULE &&
+        type[i] != mjGEOM_ELLIPSOID && type[i] != mjGEOM_CYLINDER) {
       type[i] = mjGEOM_NONE;
     }
   }
@@ -1582,6 +1584,7 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
         nrm[2] = 0;
         processed[i] = 1;
         break;
+
       default:
         // do nothing: only sphere, capsule, ellipsoid and cylinder are processed
         break;
@@ -1630,7 +1633,23 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
   mjc_setCCDObjFlex(&obj2, f2, e2, -1);
 
   // find contacts
-  return mjc_CCDIteration(m, d, &obj1, &obj2, con, 1, margin);
+  int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, 1, margin);
+
+  // fix normals for 2D flex
+  if (ncon && !mjDISABLED(mjDSBL_NATIVECCD)) {
+    // check if either object is a 2D flex
+    int isflex2d = 0;
+    if (f1 >= 0 && m->flex_dim[f1] == 2) isflex2d = 1;
+    if (f2 >= 0 && m->flex_dim[f2] == 2) isflex2d = 1;
+
+    if (isflex2d) {
+      for (int i = 0; i < ncon; i++) {
+        mjc_fixNormal(m, d, con + i, g1, -1);
+      }
+    }
+  }
+
+  return ncon;
 }
 
 

src/engine/engine_collision_driver.c

@@ -1944,6 +1944,24 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
                                vertxpos + 3*edata[2], m->flex_radius[f]);
   }
 
+  // box : triangle
+  else if (type == mjGEOM_BOX && dim == 2) {
+    const mjtNum* vertxpos = d->flexvert_xpos + 3 * m->flex_vertadr[f];
+    num = mjraw_BoxTriangle(con, margin, d->geom_xpos + 3 * g,
+                            d->geom_xmat + 9 * g, m->geom_size + 3 * g,
+                            vertxpos + 3 * edata[0], vertxpos + 3 * edata[1],
+                            vertxpos + 3 * edata[2], m->flex_radius[f]);
+  }
+
+  // capsule : triangle
+  else if (type == mjGEOM_CAPSULE && dim == 2) {
+    const mjtNum* vertxpos = d->flexvert_xpos + 3 * m->flex_vertadr[f];
+    num = mjraw_CapsuleTriangle(
+        con, margin, d->geom_xpos + 3 * g, d->geom_xmat + 9 * g,
+        m->geom_size + 3 * g, vertxpos + 3 * edata[0], vertxpos + 3 * edata[1],
+        vertxpos + 3 * edata[2], m->flex_radius[f]);
+  }
+
   // general geom : elem
   else {
     num = mjc_ConvexElem(m, d, con, g, -1, -1, -1, f, e, margin);

src/engine/engine_collision_primitive.c

@@ -606,3 +606,161 @@ 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,
+                      const mjtNum* t2, const mjtNum* t3, mjtNum rt) {
+  int cnt = 0;
+  const mjtNum* vert[3] = {t1, t2, t3};
+
+  for (int i = 0; i < 3; i++) {
+    // map vertex to box local frame
+    mjtNum diff[3], local[3];
+    mju_sub3(diff, vert[i], pos);
+    mju_mulMatTVec3(local, mat, diff);
+
+    // find max penetration / closest face
+    int maxaxis = 0;
+    mjtNum maxval = mju_abs(local[0]) - size[0];
+    for (int j = 1; j < 3; j++) {
+      mjtNum val = mju_abs(local[j]) - size[j];
+      if (val > maxval) {
+        maxval = val;
+        maxaxis = j;
+      }
+    }
+
+    // contact distance: dist = maxval - rt
+    // strictly, we only care if dist < margin
+    if (maxval - rt > margin) {
+      continue;
+    }
+
+    // check if within other dimensions (with margin/radius)
+    int inside = 1;
+    for (int j = 0; j < 3; j++) {
+      if (mju_abs(local[j]) > size[j] + margin + rt) {
+        inside = 0;
+        break;
+      }
+    }
+    if (!inside) {
+      continue;
+    }
+
+    // create contact
+    if (cnt < mjMAXCONPAIR) {
+      // normal in local frame
+      mjtNum nrm_local[3] = {0, 0, 0};
+      nrm_local[maxaxis] = (local[maxaxis] > 0 ? 1 : -1);
+
+      // normal in global frame (from Box to Triangle)
+      mju_mulMatVec3(con[cnt].frame, mat, nrm_local);
+
+      // distance
+      con[cnt].dist = maxval - rt;
+
+      // position: v - nrm * (rt + dist/2)
+      mjtNum offset = rt + con[cnt].dist * 0.5;
+      mji_addScl3(con[cnt].pos, vert[i], con[cnt].frame, -offset);
+
+      // frame details
+      mju_zero3(con[cnt].frame + 3);
+
+      cnt++;
+    }
+  }
+
+  // check box corners against triangle
+  for (int i = 0; i < 8; i++) {
+    if (cnt >= mjMAXCONPAIR) {
+      break;
+    }
+
+    // get corner in local coordinates
+    mjtNum vec[3];
+    vec[0] = (i & 1 ? size[0] : -size[0]);
+    vec[1] = (i & 2 ? size[1] : -size[1]);
+    vec[2] = (i & 4 ? size[2] : -size[2]);
+
+    // get corner in global coordinates relative to box center
+    mjtNum corner[3];
+    mju_mulMatVec3(corner, mat, vec);
+    mju_addTo3(corner, pos);
+
+    // check collision with triangle (radius 0 for corner)
+    if (mjraw_SphereTriangle(con + cnt, margin, corner, 0, t1, t2, t3, rt)) {
+      // mjraw_SphereTriangle normal points from Sphere (Corner) to Triangle.
+      cnt++;
+    }
+  }
+
+  return cnt;
+}
+
+// capsule : triangle with radius
+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) {
+  int cnt = 0;
+  mjtNum radius = size[0];
+  mjtNum len = size[1];
+  mjtNum axis[3] = {mat[2], mat[5], mat[8]};
+  mjtNum p1[3], p2[3];
+
+  // capsule endpoints
+  mju_addScl3(p1, pos, axis, -len);
+  mju_addScl3(p2, pos, axis, len);
+
+  // Check endpoints against triangle
+  cnt += mjraw_SphereTriangle(con + cnt, margin, p1, radius, t1, t2, t3, rt);
+  if (cnt >= mjMAXCONPAIR) return cnt;
+  cnt += mjraw_SphereTriangle(con + cnt, margin, p2, radius, t1, t2, t3, rt);
+  if (cnt >= mjMAXCONPAIR) return cnt;
+
+  // Check triangle vertices against capsule axis (Point-Segment)
+  const mjtNum* vert[3] = {t1, t2, t3};
+  for (int i = 0; i < 3; i++) {
+    // point-segment distance
+    mjtNum vec[3], ab[3];
+    mju_sub3(vec, vert[i], p1);
+    mju_sub3(ab, p2, p1);
+    mjtNum t = mju_dot3(vec, ab) / (4 * len * len);  // ab length is 2*len
+
+    // clamp t to [0, 1] segment (only process interior)
+    if (t <= mjMINVAL || t >= 1 - mjMINVAL) {
+      continue;
+    }
+
+    // closest point on segment
+    mjtNum closest[3];
+    mji_addScl3(closest, p1, ab, t);
+
+    // distance vector
+    mju_sub3(vec, vert[i], closest);
+    mjtNum dist = mju_normalize3(vec);
+
+    if (dist > radius + rt + margin) {
+      continue;
+    }
+
+    // con->dist
+    con[cnt].dist = dist - radius - rt;
+
+    // Frame: normal from Capsule to Triangle. 'vec' points Closest->Vert.
+    mji_copy3(con[cnt].frame, vec);
+    mju_zero3(con[cnt].frame + 3);
+
+    // Position: midway between surfaces
+    mji_add3(con[cnt].pos, closest, vert[i]);
+    mji_addToScl3(con[cnt].pos, vec, radius - rt);
+    mju_scl3(con[cnt].pos, con[cnt].pos, 0.5);
+
+    cnt++;
+    if (cnt >= mjMAXCONPAIR) return cnt;
+  }
+
+  return cnt;
+}

src/engine/engine_collision_primitive.h

@@ -46,6 +46,13 @@ int mjraw_CapsuleBox    (mjContact* con, mjtNum margin,
 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,
+                          mjtNum rt);
 
 // plane collisions
 MJAPI int mjc_PlaneSphere     (const mjModel* m, const mjData* d,