contact constraint dof traversal early termination (google-deepmind#1209)

Author: thowell

mujoco_warp/_src/constraint.py

@@ -1839,6 +1839,9 @@ def _contact_pyramidal(
           da2 = dof_parentid[da2]
         da = wp.max(da1, da2)
         if is_sparse:
+          # early termination: remaining DOFs have zero Jacobian difference
+          if da1 == da2 and da1 >= 0:
+            break
           dofid = da
         else:
           dofid -= 1
@@ -2061,6 +2064,9 @@ def _contact_elliptic(
           da2 = dof_parentid[da2]
         da = wp.max(da1, da2)
         if is_sparse:
+          # early termination: remaining DOFs have zero Jacobian difference
+          if da1 == da2 and da1 >= 0:
+            break
           dofid = da
         else:
           dofid -= 1