Fix box-box SAT collider code for GCC's pedantic mode.

Also add a detailed architectural comment that might be useful in future.

PiperOrigin-RevId: 965500329
Change-Id: Ia6a2a9137f07f726838b0acb28039a394db0efa2

src/engine/engine_collision_box.c

@@ -601,6 +601,31 @@ int mjc_CapsuleBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g
 }
 
 
+// A box-box contact manifold is computed in two stages.
+//
+// Stage 1, separating-axis test: find the axis of maximum separation among the 15 candidate
+// directions (3 face normals per box, 9 cross products of edge directions). If the boxes are
+// separated by more than margin along any candidate axis there is no contact. Face axes are
+// preferred over edge axes on near-ties: a face axis yields a multi-point manifold, which the
+// solver strongly prefers over a single edge contact of nearly identical depth.
+//
+// Stage 2, manifold generation, depends on the kind of winning axis:
+//  - face axis: the owner of the face is the reference box. The face of the other (incident)
+//    box least aligned with the reference normal is clipped against the four side planes of
+//    the reference face (Sutherland-Hodgman). Every clipped vertex within the margin band
+//    becomes a contact. Depth is the distance between the surfaces along the reference
+//    normal; contact position is midway between the surfaces along the normal, so it lies
+//    inside the intersection of the margin-inflated boxes.
+//  - edge axis: the contact is at the midpoint of the closest-point pair between the two
+//    supporting edge segments, with depth measured along the separating axis.
+//
+// Every surviving clipped vertex becomes a contact, so a face manifold carries at most
+// mjBOXBOX_MAXVERT points. Reducing the patch below the clipped polygon is not worth it:
+// on stacks of plates, whose contact patch is wide relative to their thickness, dropping
+// the polygon to a four-point subset costs two to three orders of magnitude in residual
+// motion at rest, because the support polygon shrinks and its vertex subset changes from
+// step to step as the plates shift.
+
 // Rounding scales, in units of mjtNum epsilon. Supports are sums of products of box
 // extents with rotation entries, so their absolute error is proportional to the extents:
 // mjBOXBOX_SEPEPS multiplies the summed half-sizes. The rest are dimensionless.
@@ -826,7 +851,9 @@ int mjc_BoxBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, m
     axis[i2] = rot[3*i1+j];
     mju_normalize3(axis);
     if (mju_dot3(axis, pos21) < 0) {
-      mji_scl3(axis, axis, -1);
+      axis[0] = -axis[0];
+      axis[1] = -axis[1];
+      axis[2] = -axis[2];
     }
 
     // supporting edges: the box1 edge runs along e_i at a corner selected by the axis
@@ -909,9 +936,7 @@ int mjc_BoxBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, m
 
     // contact at the midpoint of the witness pair: for penetrating edges this is inside both
     // boxes; in the margin band it is midway between the two surfaces
-    mjtNum mid[3];
+    mjtNum mid[3] = {0.5*(w1[0] + w2[0]), 0.5*(w1[1] + w2[1]), 0.5*(w1[2] + w2[2])};
-    mji_add3(mid, w1, w2);
-    mji_scl3(mid, mid, 0.5);
 
     con[0].dist = dist;
     mji_mulMatVec3(tmp, mat1, mid);
@@ -988,7 +1013,6 @@ int mjc_BoxBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, m
   nvert = clipHalfPlane(nvert, &cur, &spare, 0, -1, sizeref[ax]);
   nvert = clipHalfPlane(nvert, &cur, &spare, 1, 1, sizeref[ay]);
   nvert = clipHalfPlane(nvert, &cur, &spare, 1, -1, sizeref[ay]);
-  const mjtNum (*clipped)[3] = cur;
 
   // accept vertices within the margin band, dropping near-duplicates produced by clipping
   // at polygon corners; duplicate radius is relative to the reference face scale
@@ -996,20 +1020,20 @@ int mjc_BoxBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, m
   int naccept = 0;
   mjtNum dupe2 = mjBOXBOX_DUPEPS*(sizeref[ax]*sizeref[ax] + sizeref[ay]*sizeref[ay]);
   for (int k = 0; k < nvert; k++) {
-    if (clipped[k][2] > margin) {
+    if (cur[k][2] > margin) {
       continue;
     }
     int dupe = 0;
     for (int q = 0; q < naccept; q++) {
-      mjtNum dx = accepted[q][0] - clipped[k][0];
+      mjtNum dx = accepted[q][0] - cur[k][0];
-      mjtNum dy = accepted[q][1] - clipped[k][1];
+      mjtNum dy = accepted[q][1] - cur[k][1];
       if (dx*dx + dy*dy < dupe2) {
         dupe = 1;
         break;
       }
     }
     if (!dupe) {
-      mji_copy3(accepted[naccept++], clipped[k]);
+      mji_copy3(accepted[naccept++], cur[k]);
     }
   }
   if (naccept == 0) {