Avoid floating-point cancellation in capsule-capsule collision.

In `mjraw_CapsuleCapsule`, computing the segment-segment determinant via `ma*mc - mb*mb` suffers from floating-point cancellation for parallel capsule axes in both single and double precision (see google-deepmind/mujoco_warp#1758). In addition, an absolute `mjMINVAL` threshold depends on capsule length and is exceeded by single-precision rounding across independently rotated bodies, solving `x1` and `x2` independently in the non-parallel branch introduces uncorrelated numerator cancellation errors for near-parallel axes, and computing `mc*u - mb*v` suffers from `1 - cos^2(theta)` cancellation under axial offset.

- Compute `det` via the squared norm of the cross product of `axis1` and `axis2` (`mju_dot3(cross, cross)`).
- Use a scale-invariant relative parallel threshold `det > sin2*ma*mc` (`sin2 = 1e-8`).
- Use the Binet-Cauchy identity `(axis1 x axis2)' * (axis2 x dif)` for the `x1` numerator and Ericson's sequential segment-segment solve (`x2` derived from clipped `x1`), replacing the independent solve and clipping cascade.

PiperOrigin-RevId: 996394618
Change-Id: I3a6e1d477adcebc2054dbcf724ab5d1a387d83ac

Author: thowell

doc/changelog.rst

@@ -18,6 +18,9 @@ Engine
   degrees of freedom. Other flexes keep all their contacts. Since version 3.6.0 the contacts of every flex were reduced,
   and in the midphase for all geoms of a body together, so a cloth dropped on a plane or on many static geoms in the
   world body was held by at most 50 contacts and fell through.
+- Fixed floating-point cancellation in capsule-capsule collision detection for parallel and near-parallel axes. Thanks
+  to :github:user:`ayonga` for reporting the issue in :doc:`MJWarp <mjwarp/index>` issue
+  `1758 <https://github.com/google-deepmind/mujoco_warp/issues/1758>`__.
 
 General
 ^^^^^^^

src/engine/engine_collision_primitive.c

@@ -430,33 +430,30 @@ int mjraw_CapsuleCapsule(mjPreContact* con, mjtNum margin,
   mjtNum axis2[3] = {mat2[2] * size2[1], mat2[5] * size2[1], mat2[8] * size2[1]};
   mjtNum dif[3] = {pos1[0] - pos2[0], pos1[1] - pos2[1], pos1[2] - pos2[2]};
 
-  // compute matrix coefficients and determinant
+  // compute matrix coefficients
   mjtNum ma =  mju_dot3(axis1, axis1);
   mjtNum mb = -mju_dot3(axis1, axis2);
   mjtNum mc =  mju_dot3(axis2, axis2);
   mjtNum u  = -mju_dot3(axis1, dif);
   mjtNum v  =  mju_dot3(axis2, dif);
-  mjtNum det = ma*mc - mb*mb;
+
+  // determinant via cross product to avoid cancellation for parallel axes
+  mjtNum cross[3];
+  mji_cross(cross, axis1, axis2);
+  mjtNum det = mju_dot3(cross, cross);
 
   // general configuration (non-parallel axes)
-  if (mju_abs(det) >= mjMINVAL) {
-    // find projections, clip to segments
-    mjtNum x1 = (mc*u - mb*v) / det;
-    mjtNum x2 = (ma*v - mb*u) / det;
-
-    if (x1 > 1) {
-      x1 = 1;
-      x2 = (v - mb) / mc;
-    } else if (x1 < -1) {
-      x1 = -1;
-      x2 = (v + mb) / mc;
-    }
-    if (x2 > 1) {
-      x2 = 1;
-      x1 = mju_clip((u - mb) / ma, -1, 1);
-    } else if (x2 < -1) {
-      x2 = -1;
-      x1 = mju_clip((u + mb) / ma, -1, 1);
+  const mjtNum sin2 = 1e-8;  // sin^2(0.006 deg)
+  if (det > sin2*ma*mc) {
+    // find projections, clip to segments (Ericson, Real-Time Collision Detection, 5.1.8)
+    // Binet-Cauchy identity: mc*u - mb*v = (axis1 x axis2)' * (axis2 x dif)
+    mjtNum cross_dif[3];
+    mji_cross(cross_dif, axis2, dif);
+    mjtNum x1 = mju_clip(mju_dot3(cross, cross_dif) / det, -1, 1);
+    mjtNum x2 = (v - mb*x1) / mc;
+    if (x2 > 1 || x2 < -1) {
+      x2 = mju_clip(x2, -1, 1);
+      x1 = mju_clip((u - mb*x2) / ma, -1, 1);
     }
 
     // find nearest points, do sphere-sphere test

test/engine/engine_collision_driver_test.cc

@@ -714,5 +714,63 @@ TEST_F(MjCollisionTest, Flex3DActiveLayersMidphaseDisabled) {
   }
 }
 
+TEST_F(MjCollisionTest, ParallelCapsuleCapsule) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body euler="30 45 60">
+        <geom name="a" type="capsule" size="0.01 2.0" pos="0 0.005 0"/>
+      </body>
+      <body euler="30 0 0">
+        <body euler="0 45 60">
+          <geom name="b" type="capsule" size="0.01 1.5" pos="0 -0.005 0.3"/>
+          <geom name="c" type="capsule" size="0.01 1.5" pos="0 -0.095 0.3"/>
+          <geom name="d" type="capsule" size="0.01 1.5" pos="0 -0.095 0.3" euler="0 0.1 0"/>
+          <geom name="e" type="capsule" size="0.01 1.5" pos="0 -0.015 0.3" euler="0 0.01 0"/>
+        </body>
+      </body>
+    </worldbody>
+    <contact>
+      <pair geom1="a" geom2="b"/>
+    </contact>
+  </mujoco>
+  )";
+  char error[1024];
+  MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(m.get(), NotNull()) << error;
+  MjDataPtr d = MakeData(m);
+  ASSERT_THAT(d, NotNull());
+
+  mj_forward(m.get(), d.get());
+
+  // Penetrating rotated parallel capsules (a, b) should generate 2 contacts.
+  ASSERT_EQ(d->ncon, 2);
+  EXPECT_THAT(d->contact[0].dist, MjNear(-0.01, 1e-12, 1e-5));
+  EXPECT_THAT(d->contact[1].dist, MjNear(-0.01, 1e-12, 1e-5));
+
+  // Separated rotated parallel capsules (a, c) have surface distance 0.08.
+  int a = mj_name2id(m.get(), mjOBJ_GEOM, "a");
+  int c = mj_name2id(m.get(), mjOBJ_GEOM, "c");
+  EXPECT_THAT(mj_geomDistance(m.get(), d.get(), a, c, 0.2, nullptr),
+              MjNear(0.08, 1e-12, 1e-5));
+
+  // Near-parallel (0.1 deg) capsules (a, d): surface distance 0.08, closest
+  // point on d is at its center cross-section (distance 0.01 from d's xpos).
+  int d_id = mj_name2id(m.get(), mjOBJ_GEOM, "d");
+  mjtNum fromto[6];
+  EXPECT_THAT(mj_geomDistance(m.get(), d.get(), a, d_id, 0.2, fromto),
+              MjNear(0.08, 1e-12, 1e-5));
+  EXPECT_THAT(mju_dist3(fromto + 3, d->geom_xpos + 3 * d_id),
+              MjNear(0.01, 1e-12, 1e-2));
+
+  // Near-parallel (0.01 deg) capsules (a, e): surface distance 0.0, closest
+  // point on e is at its center cross-section (distance 0.01 from e's xpos).
+  int e_id = mj_name2id(m.get(), mjOBJ_GEOM, "e");
+  EXPECT_THAT(mj_geomDistance(m.get(), d.get(), a, e_id, 0.2, fromto),
+              MjNear(0.0, 1e-12, 1e-5));
+  EXPECT_THAT(mju_dist3(fromto + 3, d->geom_xpos + 3 * e_id),
+              MjNear(0.01, 1e-12, 1e-1));
+}
+
 }  // namespace
 }  // namespace mujoco