diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index bbae7710..ac6d8a1b 100644 --- a/src/engine/engine_collision_box.c +++ b/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]; - mji_add3(mid, w1, w2); - mji_scl3(mid, mid, 0.5); + mjtNum mid[3] = {0.5*(w1[0] + w2[0]), 0.5*(w1[1] + w2[1]), 0.5*(w1[2] + w2[2])}; 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 dy = accepted[q][1] - clipped[k][1]; + mjtNum dx = accepted[q][0] - cur[k][0]; + 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) {