Fix bad contact from passing through polytope2 in nativeccd.

PiperOrigin-RevId: 742666700
Change-Id: I76034579b4e663f9abfa265ec3e2bc608f631f59

Author: kbayes

src/engine/engine_collision_gjk.c

@@ -903,6 +903,26 @@ static void rotmat(mjtNum R[9], const mjtNum axis[3]) {
 
 
 
+// return nonzero if the ray v1v2 intersects the triangle v3v4v5
+static inline int rayTriangle(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3],
+                              const mjtNum v4[3], const mjtNum v5[3]) {
+  mjtNum diff12[3], diff13[3], diff14[3], diff15[3];
+  sub3(diff12, v2, v1);
+  sub3(diff13, v3, v1);
+  sub3(diff14, v4, v1);
+  sub3(diff15, v5, v1);
+
+  mjtNum vol1 = det3(diff13, diff14, diff12);
+  mjtNum vol2 = det3(diff14, diff15, diff12);
+  mjtNum vol3 = det3(diff15, diff13, diff12);
+
+  if (vol1 >= 0 && vol2 >= 0 && vol3 >= 0) return 1;
+  if (vol1 <= 0 && vol2 <= 0 && vol3 <= 0) return -1;
+  return 0;
+}
+
+
+
 // create a polytope from a 1-simplex (returns 0 on success)
 static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
   mjtNum *v1 = status->simplex[0].vert, *v2 = status->simplex[1].vert;
@@ -970,9 +990,9 @@ static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
     return polytope3(pt, status, obj1, obj2);
   }
 
-  // check that origin is in the hexahedron
-  if (status->dist > 10*mjMINVAL && !testTetra(v1, v3, v4, v5) && !testTetra(v2, v3, v4, v5)) {
-    return mjEPA_P2_MISSING_ORIGIN;
+  // check hexahedron is convex
+  if (!rayTriangle(v1, v2, v3, v4, v5)) {
+    return mjEPA_P2_NONCONVEX;
   }
 
   for (int i = 0; i < 6; i++) {

src/engine/engine_collision_gjk.h

@@ -44,7 +44,7 @@ typedef enum {
   mjEPA_NOCONTACT           = -1,
   mjEPA_SUCCESS             = 0,
   mjEPA_P2_INVALID_FACES,
-  mjEPA_P2_MISSING_ORIGIN,
+  mjEPA_P2_NONCONVEX,
   mjEPA_P2_ORIGIN_ON_FACE,
   mjEPA_P3_BAD_NORMAL,
   mjEPA_P3_INVALID_V4,