Added sch_hull_sat to check whether we can find a separating axes between two hulls.

src/sconvcol.h

@@ -7,6 +7,7 @@ extern "C" {
 
 #include <assert.h>
 #include <stdbool.h>
+#include <float.h>
 
 #include "vectorial/simd4x4f.h"
 
@@ -14,6 +15,10 @@ inline bool sch_simd4f_equal(simd4f a, simd4f b) {
   return (simd4f_get_x(simd4f_length4_squared(simd4f_sub(a, b))) == 0.f);
 }
 
+inline bool sch_simd4f_close(simd4f a, simd4f b, float tol) {
+  return (simd4f_get_x(simd4f_length4_squared(simd4f_sub(a, b))) < tol * tol);
+}
+
 typedef struct sch_edge sch_edge;
 typedef struct sch_vert sch_vert;
 typedef struct sch_face sch_face;
@@ -42,6 +47,7 @@ struct sch_hull {
   sch_face* faces;
   sch_edge* edges;
   sch_vert* vertices;
+  simd4f center;
   int num_faces;
   int num_edges;
   int num_vertices;
@@ -53,11 +59,10 @@ struct sch_plane {
 };
 
 struct sch_face_query {
-  sch_hull* hull_A;
-  sch_hull* hull_B;
   float dist;
-  int face_idx_A;
+  int face_idx;
-  int face_idx_B;
+  sch_plane plane;
+  simd4f vert;
 };
 
 //
@@ -107,12 +112,22 @@ void sch_edge_get_dir(const sch_edge* edge, simd4f* out_dir);
 
 void sch_hull_free_memory (sch_hull* hull);
 
+void sch_hull_translate (sch_hull* hull, float x, float y, float z);
+
+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);
 
 sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4f v1);
 
 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);
+
 int sch_hull_is_vertex_concave(const sch_hull* hull, const simd4f p);
 
 int sch_hull_is_closed (const sch_hull* hull);
@@ -203,6 +218,35 @@ void sch_builder_allocate_memory (sch_hull_builder* builder, sch_hull* out_hull)
   out_hull->edges = (sch_edge*)malloc(sizeof(sch_edge) * num_edges);
 }
 
+void sch_hull_translate (sch_hull* hull, float x, float y, float z) {
+  simd4f r = simd4f_create (x, y, z, 0.f);
+  hull->center = simd4f_add (hull->center, r);
+
+  for (int vi = 0; vi < hull->num_vertices; vi++) {
+    hull->vertices[vi].p = simd4f_add (hull->vertices[vi].p, r);
+  }
+}
+
+void sch_hull_rotate (sch_hull* hull, const float radians, const simd4f axis) {
+  simd4x4f rot_mat;
+  simd4x4f_axis_rotation(&rot_mat, radians, axis);
+  simd4x4f_matrix_vector_mul(&rot_mat, &hull->center, &hull->center);
+
+  for (int vi = 0; vi < hull->num_vertices; vi++) {
+    simd4x4f_matrix_vector_mul(&rot_mat, &hull->vertices[vi].p, &hull->vertices[vi].p);
+  }
+}
+
+void sch_hull_transform (sch_hull* hull, simd4x4f mat) {
+  simd4f v_temp = hull->center;
+  simd4x4f_matrix_vector_mul(&mat, &v_temp, &hull->center);
+
+  for (int vi = 0; vi < hull->num_vertices; vi++) {
+    v_temp = hull->vertices[vi].p;
+    simd4x4f_matrix_vector_mul(&mat, &v_temp, &hull->vertices[vi].p);
+  }
+}
+
 void sch_hull_free_memory (sch_hull* hull) {
   free (hull->faces);
   hull->faces = NULL;
@@ -359,6 +403,100 @@ void sch_hull_get_support(const sch_hull* hull, simd4f normal, simd4f* out_vert)
   *out_vert = edge->vert->p;
 }
 
+float sch_query_face_directions (const sch_hull* hull_A, const sch_hull* hull_B, sch_face_query* result) {
+  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);
+    simd4f vert;
+    sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane.n), &vert);
+    float distance = sch_plane_distance(&plane, &vert);
+    if (distance > result->dist) {
+      result->dist = distance;
+      result->face_idx = fi;
+      result->plane = plane;
+      result->vert = vert;
+    }
+  }
+
+  return result->dist;
+}
+
+float sch_query_edge_directions (const sch_hull* hull_A, const sch_hull* hull_B, sch_face_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++) {
+      sch_edge* edge_A = &hull_A->edges[iAe];
+      sch_edge* edge_B = &hull_B->edges[iBe];
+      simd4f dir_A;
+      sch_edge_get_dir(edge_A, &dir_A);
+
+      simd4f dir_B;
+      sch_edge_get_dir(edge_B, &dir_B);
+
+      simd4f axis = simd4f_cross3 (dir_A, dir_B);
+      if (simd4f_dot3_scalar(dir_A, simd4f_sub(edge_A->vert->p, hull_A->center) ) < 0.f) {
+        axis = simd4f_sub(simd4f_zero(), axis);
+      }
+
+      if (simd4f_get_x(simd4f_length3_squared(axis)) < 0.001) {
+        continue;
+      }
+
+      axis = simd4f_normalize3(axis);
+
+      sch_plane plane_A;
+      plane_A.n = axis;
+      // Note: in Gregorius talk he uses the origin of edge_A. This is wrong as
+      // there are likely points in hull_A that are further out along the plane
+      // normal. We therefore use the support point of hull_A along the axis.
+      sch_hull_get_support(hull_A, plane_A.n, &plane_A.p);
+
+      simd4f vert_B;
+      sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B);
+
+      float distance = sch_plane_distance(&plane_A, &vert_B);
+      if (distance > result->dist) {
+        result->dist = distance;
+        result->plane = plane_A;
+        result->vert = vert_B;
+      }
+    }
+  }
+
+  return result->dist;
+}
+
+bool sch_hull_sat (const sch_hull* hull_A, const sch_hull* hull_B) {
+  sch_face_query query_A_B;
+  sch_query_face_directions(hull_A, hull_B, &query_A_B);
+  if (query_A_B.dist > 0.f) {
+    return true;
+  }
+
+  sch_face_query query_B_A;
+  sch_query_face_directions(hull_B, hull_A, &query_B_A);
+  if (query_B_A.dist > 0.f) {
+    return true;
+  }
+
+  sch_face_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_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);
+  } else {
+    // sch_create_edge_contact (manifold, &query_edge, &hull_A, &hull_B);
+  }
+
+  return false;
+}
+
 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++) {
@@ -482,6 +620,7 @@ void sch_create_unitbox(sch_hull* out_hull) {
   sch_builder_face_end(&builder);
 
   int hull_result = sch_builder_create_hull(&builder, out_hull);
+  out_hull->center = simd4f_create(0.f, 0.f, 0.f, 1.f);
   assert (hull_result == SchHullResultOK);
 }
 

tests/sconvcolTests.cc

@@ -34,8 +34,6 @@ TEST_CASE("Edge Get Direction", "[sconvcol]") {
   REQUIRE(1.f == simd4f_dot3_scalar(dir, dir_ref));
 }
 
-TEST_CASE("ManualCubeHull", "[sconvcol]") {}
-
 TEST_CASE("HullBuilder", "[sconvcol]") {
   sch_hull_builder builder;
 
@@ -238,3 +236,143 @@ TEST_CASE("HullBuilder", "[sconvcol]") {
     }
   }
 }
+
+TEST_CASE("UnitCubeSupport", "[sconvcol]") {
+  GIVEN("A unit cube hull") {
+    sch_hull hull;
+    sch_create_unitbox(&hull);
+
+    WHEN("Querying support for 2, 2, 2") {
+      simd4f support;
+      simd4f normal = simd4f_create(2.f, 2.f, 2.f, 1.f);
+      sch_hull_get_support(&hull, normal, &support);
+      THEN ("support is 0.5, 0.5, 0.5") {
+        simd4f reference = simd4f_create (0.5f, 0.5f, 0.5f, 1.f);
+        REQUIRE (sch_simd4f_equal(reference, support));
+      }
+    }
+
+    WHEN("Querying support for -2, -2, -2") {
+      simd4f support;
+      simd4f normal = simd4f_create(-2.f, -2.f, -2.f, 1.f);
+      sch_hull_get_support(&hull, normal, &support);
+      THEN ("support is -0.5, -0.5, -0.5") {
+        simd4f reference = simd4f_create (-0.5f, -0.5f, -0.5f, 1.f);
+        REQUIRE (sch_simd4f_equal(reference, support));
+      }
+    }
+
+    WHEN("Querying support for 0, 1, 0") {
+      simd4f support;
+      simd4f normal = simd4f_create(0.f, 1.f, 0.f, 1.f);
+      sch_hull_get_support(&hull, normal, &support);
+      THEN ("y component of support is 0.5") {
+        REQUIRE (simd4f_get_y(support) == 0.5);
+      }
+    }
+
+    sch_hull_free_memory(&hull);
+  }
+}
+
+TEST_CASE("UnitCubeTransform", "[sconvcol]") {
+  GIVEN("A unit cube hull") {
+    sch_hull hull;
+    sch_create_unitbox(&hull);
+
+    WHEN ("not transforming cube") {
+      THEN ("center is at 0,0,0") {
+        REQUIRE (sch_simd4f_equal(simd4f_create(0.f, 0.f, 0.f, 1.f), hull.center));
+      }
+    }
+
+    WHEN ("translating by 1, 2, 3") {
+      sch_hull_translate(&hull, 1.f, 2.f, 3.f);
+      THEN ("center is at 1, 2, 3") {
+        REQUIRE (sch_simd4f_equal(simd4f_create(1.f, 2.f, 3.f, 1.f), hull.center));
+      }
+
+      THEN ("is equivalent to transform by translation matrix") {
+        sch_hull hull_trans_mat;
+        sch_create_unitbox(&hull_trans_mat);
+        simd4x4f trans_mat;
+        simd4x4f_translation(&trans_mat, 1.f, 2.f, 3.f);
+        sch_hull_transform(&hull_trans_mat, trans_mat);
+
+        REQUIRE (sch_simd4f_equal(hull.center, hull_trans_mat.center));
+
+        sch_hull_free_memory(&hull_trans_mat);
+      }
+    }
+
+    WHEN ("rotating around y by 90 degrees") {
+      simd4f vert0 = simd4f_create (0.5f, -0.5f, 0.5f, 1.f);
+      REQUIRE (sch_simd4f_equal(vert0, hull.vertices[0].p));
+      simd4f vert0_rot = simd4f_create (0.5f, -0.5f, -0.5f, 1.f);
+      sch_hull_rotate(&hull, M_PI * 0.5, simd4f_create (1.f, 0.f, 0.f, 1.f));
+      THEN ("center is at 0, 0, 0") {
+        REQUIRE (sch_simd4f_equal(simd4f_create(0.f, 0.f, 0.f, 1.f), hull.center));
+        REQUIRE (sch_simd4f_close(vert0_rot, hull.vertices[0].p, 1.0e-5f));
+      }
+    }
+
+    sch_hull_free_memory(&hull);
+  }
+}
+
+TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
+  sch_hull hull_A;
+  sch_create_unitbox(&hull_A);
+
+  sch_hull hull_B;
+  sch_create_unitbox(&hull_B);
+
+  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;
+    simd4x4f_translation(
+        &translation,
+        simd4f_get_x(r),
+        simd4f_get_y(r),
+        simd4f_get_z(r));
+    sch_hull_transform(&hull_B, translation);
+
+    THEN("Boxes separated") {
+      bool separated = sch_hull_sat(&hull_A, &hull_B);
+      REQUIRE(separated);
+    }
+  }
+
+  GIVEN("Box B rotated by 45 degrees around y and translated along x by 1.1") {
+    sch_hull_rotate(&hull_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 1.f, 0.f, 1.f));
+    sch_hull_translate(&hull_B, 1.1f, 0.f, 0.f);
+
+    THEN("Boxes overlap") {
+      bool separated = sch_hull_sat(&hull_A, &hull_B);
+      REQUIRE(!separated);
+    }
+  }
+
+  GIVEN("Box A rotated by 45 deg around z, Box B rotated by 45 degrees around y and translated along x by sqrt(2)") {
+    sch_hull_rotate(&hull_A, M_PI / 180.0f * 45.f, simd4f_create(0.f, 0.f, 1.f, 1.f));
+    sch_hull_rotate(&hull_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 1.f, 0.f, 1.f));
+    sch_hull_translate(&hull_B, sqrt(2.f), 0.f, 0.f);
+
+    THEN("Boxes overlap") {
+      bool separated = sch_hull_sat(&hull_A, &hull_B);
+      REQUIRE(!separated);
+    }
+
+    WHEN("Shifted by another 0.001 along x") {
+      sch_hull_translate(&hull_B, 0.001f, 0.f, 0.f);
+
+      THEN("Boxes are separated") {
+        bool separated = sch_hull_sat(&hull_A, &hull_B);
+        REQUIRE(separated);
+      }
+    }
+  }
+
+  sch_hull_free_memory(&hull_A);
+  sch_hull_free_memory(&hull_B);
+}