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

PiperOrigin-RevId: 864500749
Change-Id: I52aed6e29569477a8a0b6b5eb55ad483f1077c0e

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;
+}