Working on edge-edge contact manifold generation. Possibly found issue for face-face contact generation.

src/sconvcol.h

@@ -28,6 +28,7 @@ typedef struct sch_hull sch_hull;
 typedef struct sch_plane sch_plane;
 typedef struct sch_hull_builder sch_hull_builder;
 typedef struct sch_face_query sch_face_query;
+typedef struct sch_edge_query sch_edge_query;
 typedef struct sch_manifold sch_manifold;
 
 struct sch_edge {
@@ -68,6 +69,13 @@ struct sch_face_query {
   simd4f vert;
 };
 
+struct sch_edge_query {
+  float dist;
+  sch_plane plane;
+  int edge_A_idx;
+  int edge_B_idx;
+};
+
 struct sch_manifold {
   int num_points;
   int num_vertices;
@@ -152,6 +160,8 @@ void sch_clip_faces (const sch_face* ref_face, const sch_face* inc_face, sch_man
 
 void sch_create_face_contact (const sch_face_query* query_A_B, const sch_hull* hull_A, const sch_face_query* query_B_A, const sch_hull* hull_B, sch_manifold *result);
 
+void sch_create_edge_contact (const sch_edge_query* query_edge, const sch_hull* hull_A, const sch_hull* hull_B, sch_manifold* result);
+
 bool sch_hull_sat(const sch_hull* hull_A, const sch_hull* hull_B, sch_manifold* result);
 
 int sch_hull_is_vertex_concave(const sch_hull* hull, const simd4f p);
@@ -497,7 +507,7 @@ float sch_query_face_directions (const sch_hull* hull_A, const sch_hull* hull_B,
   return result->dist;
 }
 
-float sch_query_edge_directions (const sch_hull* hull_A, const sch_hull* hull_B, sch_face_query* result) {
+float sch_query_edge_directions (const sch_hull* hull_A, const sch_hull* hull_B, sch_edge_query* result) {
   result->dist = -FLT_MAX;
   for (int iAe = 0; iAe < hull_A->num_edges; iAe++) {
     for (int iBe = 0; iBe < hull_B->num_edges; iBe++) {
@@ -534,7 +544,8 @@ float sch_query_edge_directions (const sch_hull* hull_A, const sch_hull* hull_B,
       if (distance > result->dist) {
         result->dist = distance;
         result->plane = plane_A;
-        result->vert = vert_B;
+        result->edge_A_idx = iAe;
+        result->edge_B_idx = iBe;
       }
     }
   }
@@ -635,6 +646,37 @@ void sch_create_face_contact (const sch_face_query* query_A_B, const sch_hull* h
   }
 }
 
+void sch_create_edge_contact (const sch_edge_query* query_edge, const sch_hull* hull_A, const sch_hull* hull_B, sch_manifold* result) {
+  sch_edge edge_A = hull_A->edges[query_edge->edge_A_idx];
+  sch_edge edge_B = hull_B->edges[query_edge->edge_B_idx];
+
+  sch_plane plane_A_n;
+  plane_A_n.p = edge_A.vert->p;
+  plane_A_n.n = simd4f_normalize3(simd4f_cross3(simd4f_sub(edge_A.vert->p, edge_A.next->vert->p), query_edge->plane.n));
+  simd4f edge_B_projected;
+  bool is_edge_B_intersecting;
+  is_edge_B_intersecting = sch_plane_intersection(&plane_A_n, &edge_B.vert->p, &edge_B.next->vert->p, &edge_B_projected);
+  assert (is_edge_B_intersecting);
+
+  sch_plane plane_B_n;
+  plane_B_n.p = edge_B.vert->p;
+  plane_B_n.n = simd4f_normalize3(simd4f_cross3(simd4f_sub(edge_B.vert->p, edge_B.next->vert->p), query_edge->plane.n));
+  simd4f edge_A_projected;
+  bool is_edge_A_intersecting;
+  is_edge_A_intersecting = sch_plane_intersection(&plane_B_n, &edge_A.vert->p, &edge_A.next->vert->p, &edge_A_projected);
+  assert (is_edge_A_intersecting);
+
+  sch_manifold_add_point(result,simd4f_add(simd4f_mul(edge_B_projected, simd4f_splat(0.5f)),
+      simd4f_mul(edge_B_projected, simd4f_splat(0.5f))));
+  result->normal = query_edge->plane.n;
+
+#ifndef NDEBUG
+  simd4f closest_points_dir = simd4f_normalize3(simd4f_sub (edge_B_projected, edge_A_projected));
+  float normal_error = 1.0f - simd4f_dot3_scalar(closest_points_dir, result->normal);
+  assert (fabs(normal_error) < SCH_EPS);
+#endif
+}
+
 bool sch_hull_sat(
     const sch_hull* hull_A,
     const sch_hull* hull_B,
@@ -651,18 +693,18 @@ bool sch_hull_sat(
     return true;
   }
 
-  sch_face_query query_edge;
+  sch_edge_query query_edge;
   sch_query_edge_directions (hull_A, hull_B, &query_edge);
   if (query_edge.dist > 0.f) {
     return true;
   }
 
-  bool is_face_contact_A = query_A_B.dist >= query_edge.dist;
+  bool is_face_contact_A = query_A_B.dist > query_edge.dist;
-  bool is_face_contact_B = query_B_A.dist >= query_edge.dist;
+  bool is_face_contact_B = query_B_A.dist > query_edge.dist;
   if (is_face_contact_A && is_face_contact_B) {
     sch_create_face_contact (&query_A_B, hull_A, &query_B_A, hull_B, result);
   } else {
-    // sch_create_edge_contact (manifold, &query_edge, &hull_A, &hull_B);
+    sch_create_edge_contact (&query_edge, hull_A, hull_B, result);
   }
 
   return false;

tests/sconvcolTests.cc

@@ -378,7 +378,17 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
 
   GIVEN("Box B translated by 1z and rotated 45 degrees around z") {
     sch_hull_rotate(&hull_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 0.f, 1.f, 1.f));
-    sch_hull_translate(&hull_B, 0., 0.f, 1.f);
+    sch_hull_translate(&hull_B, 0., 0.f, 0.97f);
+
+    THEN("Boxes overlap") {
+      bool separated = sch_hull_sat(&hull_A, &hull_B, &manifold);
+      REQUIRE(!separated);
+      REQUIRE(manifold.num_points == 8);
+    }
+  }
+
+  GIVEN("Box B translated by 1x, 1.001z") {
+    sch_hull_translate(&hull_B, 1., 0.f, 1.00f);
 
     THEN("Boxes overlap") {
       bool separated = sch_hull_sat(&hull_A, &hull_B, &manifold);