Added face-face contact manifold computation to sconvcol.

src/sconvcol.h

@@ -11,6 +11,8 @@ extern "C" {
 
 #include "vectorial/simd4x4f.h"
 
+#define SCH_EPS 0.001
+
 inline bool sch_simd4f_equal(simd4f a, simd4f b) {
   return (simd4f_get_x(simd4f_length4_squared(simd4f_sub(a, b))) == 0.f);
 }
@@ -26,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_manifold sch_manifold;
 
 struct sch_edge {
   sch_vert* vert;
@@ -65,6 +68,13 @@ struct sch_face_query {
   simd4f vert;
 };
 
+struct sch_manifold {
+  int num_points;
+  int num_vertices;
+  simd4f* vertices;
+  simd4f normal;
+};
+
 //
 // Hull Builder
 //
@@ -102,12 +112,24 @@ void sch_builder_face_end(sch_hull_builder* builder);
 
 SchHullResult sch_builder_create_hull(sch_hull_builder* builder, sch_hull* out_hull);
 
+//
+// Helper data structures
+//
+void sch_manifold_alloc (sch_manifold* manifold, int num_vertices);
+void sch_manifold_add_point (sch_manifold* manifold, simd4f p);
+void sch_manifold_reset (sch_manifold* manifold);
+void sch_manifold_free_memory (sch_manifold* manifold);
+
 //
 // Calculations
 //
 
 float sch_plane_distance(const sch_plane* plane, const simd4f* v);
 
+bool sch_plane_intersection(const sch_plane* plane, const simd4f* p0, const simd4f* p1, simd4f* result);
+
+void sch_plane_point_project(const sch_plane* plane, simd4f* p);
+
 void sch_edge_get_dir(const sch_edge* edge, simd4f* out_dir);
 
 void sch_hull_free_memory (sch_hull* hull);
@@ -118,7 +140,7 @@ void sch_hull_rotate (sch_hull* hull, const float radians, const simd4f axis);
 
 void sch_hull_transform (sch_hull* hull, simd4x4f mat);
 
-void sch_hull_get_plane(const sch_hull* hull, const int index, sch_plane* out_plane);
+void sch_hull_calc_plane(const sch_hull* hull, const int index, sch_plane* out_plane);
 
 sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4f v1);
 
@@ -126,7 +148,11 @@ void sch_hull_get_support(const sch_hull* hull, simd4f n, simd4f* out_vert);
 
 float sch_query_face_directions (const sch_hull* hull_A, const sch_hull* hull_B, sch_face_query* result);
 
-bool sch_hull_sat (const sch_hull* hullA, const sch_hull* hullB);
+void sch_clip_faces (const sch_face* ref_face, const sch_face* inc_face, sch_manifold* manifold);
+
+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);
+
+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);
 
@@ -144,10 +170,53 @@ void sch_create_unitbox(sch_hull* out_hull);
 
 #ifdef SCONVCOL_IMPLEMENTATION
 
+void sch_manifold_alloc (sch_manifold* manifold, int num_vertices) {
+  manifold->vertices = (simd4f*) aligned_alloc(16, sizeof(simd4f) * num_vertices);
+  assert (manifold->vertices != NULL);
+  manifold->num_vertices = num_vertices;
+}
+
+void sch_manifold_add_point (sch_manifold* manifold, simd4f p) {
+  assert (manifold->num_vertices > manifold->num_points);
+  manifold->vertices[manifold->num_points] = p;
+  manifold->num_points++;
+}
+
+void sch_manifold_reset (sch_manifold* manifold) {
+  manifold->num_points = 0;
+}
+
+void sch_manifold_free_memory (sch_manifold* manifold) {
+  free (manifold->vertices);
+  manifold->vertices = NULL;
+  manifold->num_vertices = 0;
+}
+
 float sch_plane_distance(const sch_plane* plane, const simd4f* v) {
   return simd4f_dot3_scalar(simd4f_sub(*v, plane->p), plane->n);
 }
 
+bool sch_plane_intersection(const sch_plane* plane, const simd4f* p0, const simd4f* p1, simd4f* result) {
+  simd4f line = simd4f_sub (*p1, *p0);
+  float line_dot_n = simd4f_dot3_scalar(plane->n, line);
+  if (fabs(line_dot_n) < SCH_EPS) {
+    return false;
+  }
+
+  float s = -simd4f_dot3_scalar(simd4f_sub(*p0, plane->p), plane->n) / line_dot_n;
+  *result = simd4f_add(*p0, simd4f_mul(line, simd4f_splat(s)));
+  if (fabs (sch_plane_distance(plane, result)) <= SCH_EPS) {
+    return true;
+  }
+
+  return false;
+}
+
+void sch_plane_point_project(const sch_plane* plane, simd4f* p) {
+  float s = -simd4f_dot3_scalar(simd4f_sub(*p, plane->p), plane->n);
+  *p = simd4f_add (*p, simd4f_mul(plane->n, simd4f_splat(s)));
+}
+
 void sch_create_face(int num_vert, simd4f* vertices, sch_face* out_face) {
   assert(out_face != NULL);
   assert(out_face->edge == NULL);
@@ -348,13 +417,10 @@ void sch_builder_face_vertex(sch_hull_builder* builder, simd4f vertex) {
   builder->face_vert_idx[face_end_idx] = vert_index;
 }
 
-void sch_hull_get_plane(const sch_hull* hull, const int index, sch_plane* out_plane) {
+void sch_face_calc_plane(const sch_face* face, sch_plane* out_plane) {
-  assert(hull != NULL);
+  assert (face != NULL);
-  assert(index >= 0 && index < hull->num_faces);
+  assert (out_plane != NULL);
-  assert(out_plane != NULL);
 
-  // TODO move plane calculation to create hull?
-  sch_face* face = &hull->faces[index];
   sch_edge* edge0 = face->edge;
   sch_edge* edge1 = edge0->next;
   simd4f dir0;
@@ -366,6 +432,15 @@ void sch_hull_get_plane(const sch_hull* hull, const int index, sch_plane* out_pl
   out_plane->n = simd4f_cross3(dir0, dir1);
 }
 
+void sch_hull_calc_plane(const sch_hull* hull, const int index, sch_plane* out_plane) {
+  assert(hull != NULL);
+  assert(index >= 0 && index < hull->num_faces);
+  assert(out_plane != NULL);
+
+  // TODO move plane calculation to create hull?
+  sch_face_calc_plane(&hull->faces[index], out_plane);
+}
+
 sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4f v1) {
   for (int fi = 0; fi < hull->num_faces; fi++) {
     sch_face* face = &hull->faces[fi];
@@ -407,7 +482,7 @@ float sch_query_face_directions (const sch_hull* hull_A, const sch_hull* hull_B,
   result->dist = -FLT_MAX;
   for (int fi = 0; fi < hull_A->num_faces; fi++) {
     sch_plane plane;
-    sch_hull_get_plane(hull_A, fi, &plane);
+    sch_hull_calc_plane(hull_A, fi, &plane);
     simd4f vert;
     sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane.n), &vert);
     float distance = sch_plane_distance(&plane, &vert);
@@ -439,7 +514,7 @@ float sch_query_edge_directions (const sch_hull* hull_A, const sch_hull* hull_B,
         axis = simd4f_sub(simd4f_zero(), axis);
       }
 
-      if (simd4f_get_x(simd4f_length3_squared(axis)) < 0.001) {
+      if (simd4f_get_x(simd4f_length3_squared(axis)) < SCH_EPS) {
         continue;
       }
 
@@ -467,7 +542,103 @@ float sch_query_edge_directions (const sch_hull* hull_A, const sch_hull* hull_B,
   return result->dist;
 }
 
-bool sch_hull_sat (const sch_hull* hull_A, const sch_hull* hull_B) {
+void sch_clip_faces (const sch_face* ref_face, const sch_face* inc_face, sch_manifold* manifold) {
+  simd4f* input_vertices = malloc(sizeof(simd4f) * manifold->num_vertices);
+  assert (input_vertices != NULL);
+
+  sch_edge* inc_start_edge = inc_face->edge;
+  sch_edge* inc_cur_edge = inc_face->edge;
+
+  sch_manifold_reset(manifold);
+  int num_input_vertices = 0;
+  do {
+    sch_manifold_add_point(manifold, inc_cur_edge->vert->p);
+    inc_cur_edge = inc_cur_edge->next;
+  } while (inc_cur_edge != inc_start_edge);
+
+  sch_edge* ref_start_edge = ref_face->edge;
+  sch_edge* ref_cur_edge = ref_face->edge;
+
+  sch_plane ref_face_plane;
+  sch_face_calc_plane(ref_face, &ref_face_plane);
+
+  do {
+    memcpy(input_vertices, manifold->vertices, sizeof(simd4f) * manifold->num_points);
+    num_input_vertices = manifold->num_points;
+    sch_manifold_reset(manifold);
+
+    // construct plane orthogonal to reference face that contains current edge.
+    sch_plane edge_plane;
+    edge_plane.n = simd4f_normalize3(simd4f_cross3(ref_face_plane.n, simd4f_sub(ref_cur_edge->next->vert->p, ref_cur_edge->vert->p)));
+    edge_plane.p = ref_cur_edge->vert->p;
+
+    for (int i = 0; i < num_input_vertices; i++) {
+      simd4f current = input_vertices[i];
+      simd4f prev = input_vertices[(i + num_input_vertices - 1) % num_input_vertices];
+      simd4f intersection;
+      bool is_intersecting = sch_plane_intersection(&edge_plane, &current, &prev, &intersection);
+
+      if (sch_plane_distance(&edge_plane, &current) >= SCH_EPS) {
+        if (sch_plane_distance(&edge_plane, &prev) <= SCH_EPS) {
+          sch_manifold_add_point(manifold, intersection);
+        }
+        sch_manifold_add_point(manifold, current);
+      } else if (sch_plane_distance(&edge_plane, &prev) >= SCH_EPS) {
+        sch_manifold_add_point(manifold, intersection);
+      }
+    }
+
+    ref_cur_edge = ref_cur_edge->next;
+  } while (ref_cur_edge != ref_start_edge);
+
+  free(input_vertices);
+}
+
+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) {
+  const sch_plane* ref_plane = &query_A_B->plane;
+  int ref_face_idx = query_A_B->face_idx;
+  const sch_hull* ref_hull = hull_A;
+  const sch_hull* inc_hull = hull_B;
+  const sch_face* ref_face = &hull_A->faces[query_A_B->face_idx];
+
+  // normalize input
+  if (query_A_B->dist < query_B_A->dist) {
+    ref_plane = &query_B_A->plane;
+    ref_face_idx = query_B_A->face_idx;
+    ref_face = &hull_B->faces[query_B_A->face_idx];
+    ref_hull = hull_B;
+    inc_hull = hull_A;
+  }
+
+  result->normal = ref_plane->n;
+
+  // find most antiparallel face
+  float dot_min = 1.0f;
+  int inc_face_idx = -1;
+  for (int i = 0; i < inc_hull->num_faces; i++) {
+    sch_plane face_plane;
+    sch_hull_calc_plane(inc_hull, i, &face_plane);
+    float normal_dot = simd4f_dot3_scalar(result->normal, face_plane.n);
+    if (normal_dot < dot_min) {
+      dot_min = normal_dot;
+      inc_face_idx = i;
+    }
+  }
+  assert (inc_face_idx >= 0);
+
+  const sch_face* inc_face = &inc_hull->faces[inc_face_idx];
+  sch_clip_faces(ref_face, inc_face, result);
+
+  // project points onto plane of reference face
+  for (int i = 0; i < result->num_points; i++) {
+    sch_plane_point_project(ref_plane, &result->vertices[i]);
+  }
+}
+
+bool sch_hull_sat(
+    const sch_hull* hull_A,
+    const sch_hull* hull_B,
+    sch_manifold* result) {
   sch_face_query query_A_B;
   sch_query_face_directions(hull_A, hull_B, &query_A_B);
   if (query_A_B.dist > 0.f) {
@@ -486,10 +657,10 @@ bool sch_hull_sat (const sch_hull* hull_A, const sch_hull* hull_B) {
     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 (manifold, &query_A_B, &hull_A, &query_B_A, &hull_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);
   }
@@ -500,7 +671,7 @@ bool sch_hull_sat (const sch_hull* hull_A, const sch_hull* hull_B) {
 int sch_hull_is_vertex_concave(const sch_hull* hull, const simd4f v) {
   sch_plane plane;
   for (int i = 0; i < hull->num_faces; i++) {
-    sch_hull_get_plane(hull, i, &plane);
+    sch_hull_calc_plane(hull, i, &plane);
     float distance = sch_plane_distance(&plane, &v);
     return (distance > 0.);
   }
@@ -595,7 +766,7 @@ void sch_create_unitbox(sch_hull* out_hull) {
   sch_builder_face_vertex(&builder, simd4f_create (-0.5f, 0.5f,  0.5f, 1.f));
   sch_builder_face_end(&builder);
 
-  // +y
+  // -y
   sch_builder_face_begin(&builder);
   sch_builder_face_vertex(&builder, simd4f_create (-0.5f, -0.5f, -0.5f, 1.f));
   sch_builder_face_vertex(&builder, simd4f_create ( 0.5f, -0.5f, -0.5f, 1.f));

tests/sconvcolTests.cc

@@ -327,6 +327,9 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
   sch_hull hull_B;
   sch_create_unitbox(&hull_B);
 
+  sch_manifold manifold;
+  sch_manifold_alloc(&manifold, SCH_BUILDER_MAX_NUM_FACE_VERTICES);
+
   GIVEN ("Two unit boxes separated by 1.1 along x axis") {
     simd4f r = simd4f_create(0.5f + 0.4f * sqrtf(2.), 0.f, 0.f, 1.f);
     simd4x4f translation;
@@ -338,7 +341,7 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
     sch_hull_transform(&hull_B, translation);
 
     THEN("Boxes separated") {
-      bool separated = sch_hull_sat(&hull_A, &hull_B);
+      bool separated = sch_hull_sat(&hull_A, &hull_B, &manifold);
       REQUIRE(separated);
     }
   }
@@ -348,7 +351,7 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
     sch_hull_translate(&hull_B, 1.1f, 0.f, 0.f);
 
     THEN("Boxes overlap") {
-      bool separated = sch_hull_sat(&hull_A, &hull_B);
+      bool separated = sch_hull_sat(&hull_A, &hull_B, &manifold);
       REQUIRE(!separated);
     }
   }
@@ -359,7 +362,7 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
     sch_hull_translate(&hull_B, sqrt(2.f), 0.f, 0.f);
 
     THEN("Boxes overlap") {
-      bool separated = sch_hull_sat(&hull_A, &hull_B);
+      bool separated = sch_hull_sat(&hull_A, &hull_B, &manifold);
       REQUIRE(!separated);
     }
 
@@ -367,12 +370,156 @@ TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
       sch_hull_translate(&hull_B, 0.001f, 0.f, 0.f);
 
       THEN("Boxes are separated") {
-        bool separated = sch_hull_sat(&hull_A, &hull_B);
+        bool separated = sch_hull_sat(&hull_A, &hull_B, &manifold);
         REQUIRE(separated);
       }
     }
   }
 
+  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);
+
+    THEN("Boxes overlap") {
+      bool separated = sch_hull_sat(&hull_A, &hull_B, &manifold);
+      REQUIRE(!separated);
+      REQUIRE(manifold.num_points == 8);
+    }
+  }
+
+  sch_manifold_free_memory(&manifold);
   sch_hull_free_memory(&hull_A);
   sch_hull_free_memory(&hull_B);
 }
+
+TEST_CASE("PlaneTests", "[sconvcol]") {
+  GIVEN("Z=0 Plane") {
+    sch_plane plane;
+    plane.n = simd4f_create(0.f, 0.f, 1.f, 1.f);
+    plane.p = simd4f_create(1.f, 1.2354f, 0.f, 1.f);
+
+    WHEN("Querying point with z = -0.1") {
+      simd4f p = simd4f_create (1231.3f, 552.2f, -0.1f, 1.f);
+      float dist = sch_plane_distance (&plane, &p);
+      THEN ("distance is -0.1") {
+        REQUIRE (dist == -0.1f);
+      }
+    }
+
+    WHEN ("Intersecting with line (5., 3., 2.) - (12., 332., SCH_EPS") {
+      simd4f p0 = simd4f_create (5.f, 3.f, 1.f, 1.f);
+      simd4f p1 = simd4f_create (12.f, 332.f, SCH_EPS, 1.f);
+      simd4f result;
+      bool is_intersecting = sch_plane_intersection(&plane, &p0, &p1, &result);
+      THEN ("have intersection") {
+        REQUIRE (is_intersecting);
+      }
+
+      WHEN ("Reversing the query") {
+        simd4f result;
+        bool is_intersecting = sch_plane_intersection(&plane, &p1, &p0, &result);
+        THEN ("have intersection") {
+          REQUIRE (is_intersecting);
+        }
+      }
+    }
+
+    WHEN ("Intersecting with line (5., 3., 2.) - (12., 332., SCH_EPS * 2") {
+      simd4f p0 = simd4f_create (5.f, 3.f, 1.f, 1.f);
+      simd4f p1 = simd4f_create (12.f, 332.f, SCH_EPS * 2.f, 1.f);
+      simd4f result;
+      bool is_intersecting = sch_plane_intersection(&plane, &p0, &p1, &result);
+      THEN ("have no intersection") {
+        REQUIRE (is_intersecting);
+      }
+
+      WHEN ("Reversing the query") {
+        simd4f result;
+        bool is_intersecting = sch_plane_intersection(&plane, &p1, &p0, &result);
+        THEN ("have no intersection") {
+          REQUIRE (is_intersecting);
+        }
+      }
+    }
+
+    WHEN("Projecting point (123.4, 332., 212.) onto plane") {
+      simd4f p = simd4f_create (123.4f, 332.f, 212.f, 1.f);
+      sch_plane_point_project(&plane, &p);
+      THEN ("z component is 0.") {
+        REQUIRE (simd4f_get_z(p) == 0.f);
+      }
+
+      WHEN("Using z = -212.0") {
+        p = simd4f_create(123.4f, 332.f, -212.f, 1.f);
+        sch_plane_point_project(&plane, &p);
+        THEN("projected z component is 0.") {
+          REQUIRE(simd4f_get_z(p) == 0.f);
+        }
+      }
+    }
+  }
+}
+
+TEST_CASE ("ClipFaces", "[sconvcol]") {
+  sch_hull_builder builder;
+  sch_hull triangle_hull;
+  sch_hull quad_hull;
+
+  // Build triangle
+  sch_builder_reset(&builder);
+
+  // top
+  sch_builder_face_begin(&builder);
+  sch_builder_face_vertex(&builder, simd4f_create (0.5f, -0.5f, 0.f, 1.f));
+  sch_builder_face_vertex(&builder, simd4f_create ( 0.0f, 0.5f, 0.f, 1.f));
+  sch_builder_face_vertex(&builder, simd4f_create ( -0.5f, -0.5f,  0.f, 1.f));
+
+  sch_builder_face_end(&builder);
+
+  // bottom
+  sch_builder_face_begin(&builder);
+  sch_builder_face_vertex(&builder, simd4f_create (0.5f, -0.5f, 0.f, 1.f));
+  sch_builder_face_vertex(&builder, simd4f_create ( -0.5f, -0.5f,  0.f, 1.f));
+  sch_builder_face_vertex(&builder, simd4f_create ( 0.0f, 0.5f, 0.f, 1.f));
+  sch_builder_face_end(&builder);
+
+  int hull_result = sch_builder_create_hull(&builder, &triangle_hull);
+  triangle_hull.center = simd4f_create(0.f, 0.f, 0.f, 1.f);
+  REQUIRE (hull_result == SchHullResultOK);
+
+  // Build quad
+  sch_builder_reset(&builder);
+
+  // top
+  sch_builder_face_begin(&builder);
+  sch_builder_face_vertex(&builder, simd4f_create (-0.5f, -0.5f, 0.f, 1.f));
+  sch_builder_face_vertex(&builder, simd4f_create ( 0.5f, -0.5f, 0.f, 1.f));
+  sch_builder_face_vertex(&builder, simd4f_create ( 0.5f, 0.5f,  0.f, 1.f));
+  sch_builder_face_vertex(&builder, simd4f_create (-0.5f, 0.5f,  0.f, 1.f));
+  sch_builder_face_end(&builder);
+
+  // bottom
+  sch_builder_face_begin(&builder);
+  sch_builder_face_vertex(&builder, simd4f_create(0.5f, -0.5f, 0.f, 1.f));
+  sch_builder_face_vertex(&builder, simd4f_create(-0.5f, -0.5f, 0.f, 1.f));
+  sch_builder_face_vertex(&builder, simd4f_create(-0.5f, 0.5f, 0.f, 1.f));
+  sch_builder_face_vertex(&builder, simd4f_create(0.5f, 0.5f, 0.f, 1.f));
+  sch_builder_face_end(&builder);
+
+  hull_result = sch_builder_create_hull(&builder, &quad_hull);
+  quad_hull.center = simd4f_create(0.f, 0.f, 0.f, 1.f);
+  REQUIRE (hull_result == SchHullResultOK);
+
+  GIVEN ("A Triangle and a Quad") {
+    WHEN ("Translating the Quad along +Y") {
+      sch_hull_translate(&quad_hull, 0.f, 0.5f, 0.f);
+      sch_manifold manifold;
+      sch_manifold_alloc(&manifold, SCH_BUILDER_MAX_NUM_FACE_VERTICES);
+      sch_manifold_reset(&manifold);
+
+      sch_clip_faces(&triangle_hull.faces[0], &quad_hull.faces[1], &manifold);
+      REQUIRE (manifold.num_points == 3);
+      sch_manifold_free_memory(&manifold);
+    }
+  }
+}