Check vor concave vertices when building convex hull objects

src/sconvcol.h

@@ -62,6 +62,13 @@ struct sch_face_query {
 #define SCH_BUILDER_MAX_NUM_FACES 256
 #define SCH_BUILDER_MAX_NUM_FACE_VERTICES 32
 
+enum SchHullResult {
+  SchHullResultOK = 0,
+  SchHullResultConcaveVertex = -1
+};
+
+typedef enum SchHullResult SchHullResult;
+
 struct sch_hull_builder {
   int num_vertices;
   int num_faces;
@@ -79,7 +86,7 @@ void sch_builder_face_vertex(sch_hull_builder* builder, simd4f vertex);
 
 void sch_builder_face_end(sch_hull_builder* builder);
 
-void sch_builder_create_hull(sch_hull_builder* builder, sch_hull* out_hull);
+SchHullResult sch_builder_create_hull(sch_hull_builder* builder, sch_hull* out_hull);
 
 //
 // Calculations
@@ -89,13 +96,17 @@ float sch_plane_distance(sch_plane* plane, simd4f* v);
 
 void sch_edge_get_dir(sch_edge* edge, simd4f* dir);
 
+void sch_hull_free_memory (sch_hull* hull);
+
 void sch_hull_get_plane(const sch_hull* hull, int index, sch_plane* out_plane);
 
 void sch_hull_get_support(const sch_hull* hull, simd4f n, simd4f* out_vert);
 
+int sch_hull_is_vertex_concave(const sch_hull* hull, simd4f p);
+
 void sch_create_face(int num_vert, simd4f* vertices, sch_face* out_face);
 
-void sch_create_hull(int num_faces, sch_face* faces, sch_hull* out_hull);
+SchHullResult sch_create_hull(int num_faces, sch_face* faces, sch_hull* out_hull);
 
 void sch_query_face_dir(
     const sch_hull* hull_A,
@@ -157,15 +168,14 @@ void sch_builder_face_begin(sch_hull_builder* builder) {
 
 void sch_builder_face_end(sch_hull_builder* builder) { builder->num_faces++; }
 
-void sch_builder_create_hull(sch_hull_builder* builder, sch_hull* out_hull) {
+void sch_builder_allocate_memory (sch_hull_builder* builder, sch_hull* out_hull) {
-  out_hull->num_faces = builder->num_faces;
   out_hull->faces = (sch_face*)malloc(sizeof(sch_face) * builder->num_faces);
 
   out_hull->vertices =
       (sch_vert*)malloc(sizeof(sch_vert) * builder->num_vertices);
 
   int num_edges = 1;
-  for (int face_index = 0; face_index < out_hull->num_faces; face_index++) {
+  for (int face_index = 0; face_index < builder->num_faces; face_index++) {
     int start_idx = builder->face_vert_idx_start[face_index];
     int end_idx = builder->face_vert_idx_end[face_index];
     int edge_count = end_idx - start_idx;
@@ -174,9 +184,25 @@ void sch_builder_create_hull(sch_hull_builder* builder, sch_hull* out_hull) {
   }
 
   out_hull->edges = (sch_edge*)malloc(sizeof(sch_edge) * num_edges);
+}
+
+void sch_hull_free_memory (sch_hull* hull) {
+  free (hull->faces);
+  hull->faces = NULL;
+  free (hull->vertices);
+  hull->vertices = NULL;
+  free (hull->edges);
+  hull->edges = NULL;
+}
+
+SchHullResult sch_builder_create_hull(sch_hull_builder* builder, sch_hull* out_hull) {
+  sch_builder_allocate_memory(builder, out_hull);
+
   int hull_edge_idx = 0;
 
-  for (int face_index = 0; face_index < out_hull->num_faces; face_index++) {
+  out_hull->num_faces = 0;
+
+  for (int face_index = 0; face_index < builder->num_faces; face_index++) {
     sch_face* face = &out_hull->faces[face_index];
 
     int start_idx = builder->face_vert_idx_start[face_index];
@@ -194,12 +220,18 @@ void sch_builder_create_hull(sch_hull_builder* builder, sch_hull* out_hull) {
 
       int vert_idx = builder->face_vert_idx[face_vert_idx];
       sch_vert* vert = &out_hull->vertices[vert_idx];
+
       vert->p = builder->vertices[vert_idx];
       vert->edge = edge;
       edge->vert = vert;
       edge->twin = NULL;
       edge->face = face;
 
+      if (sch_hull_is_vertex_concave(out_hull, vert->p)) {
+        sch_hull_free_memory(out_hull);
+        return SchHullResultConcaveVertex;
+      }
+
       if (face_vert_idx == end_idx) {
         edge->next = face->edge;
       }
@@ -209,7 +241,11 @@ void sch_builder_create_hull(sch_hull_builder* builder, sch_hull* out_hull) {
 
       prev_edge = edge;
     }
+
+    out_hull->num_faces = face_index + 1;
   }
+
+  return SchHullResultOK;
 }
 
 void sch_builder_face_vertex(sch_hull_builder* builder, simd4f vertex) {
@@ -241,13 +277,6 @@ void sch_builder_face_vertex(sch_hull_builder* builder, simd4f vertex) {
   builder->face_vert_idx[face_end_idx] = vert_index;
 }
 
-void sch_create_hull(int num_faces, sch_face* faces, sch_hull* out_hull) {
-  assert(out_hull != NULL);
-
-  out_hull->num_faces = num_faces;
-  out_hull->faces = faces;
-}
-
 void sch_hull_get_plane(const sch_hull* hull, int index, sch_plane* out_plane) {
   assert(hull != NULL);
   assert(index >= 0 && index < hull->num_faces);
@@ -266,6 +295,53 @@ void sch_hull_get_plane(const sch_hull* hull, int index, sch_plane* out_plane) {
   out_plane->n = simd4f_cross3(dir0, dir1);
 }
 
+int sch_hull_is_vertex_concave(const sch_hull* hull, simd4f v) {
+  sch_plane plane;
+  for (int i = 0; i < hull->num_faces; i++) {
+    sch_hull_get_plane(hull, i, &plane);
+    float distance = sch_plane_distance(&plane, &v);
+    return (distance > 0.);
+  }
+
+  return 0;
+}
+
+sch_hull* sch_create_box() {
+  simd4f vertices_pos_z[4] = {
+      simd4f_create(-0.5f, -0.5f, 0.5f, 0.f),
+      simd4f_create(0.5f, -0.5f, 0.5f, 0.f),
+      simd4f_create(0.5f, 0.5f, 0.5f, 0.f),
+      simd4f_create(-0.5f, 0.5f, 0.5f, 0.f)};
+  sch_face face_pos_z = {0};
+  sch_create_face(4, vertices_pos_z, &face_pos_z);
+
+  simd4f vertices_neg_z[4] = {
+      simd4f_create(-0.5f, -0.5f, -0.5f, 0.f),
+      simd4f_create(0.5f, -0.5f, -0.5f, 0.f),
+      simd4f_create(0.5f, 0.5f, -0.5f, 0.f),
+      simd4f_create(-0.5f, 0.5f, -0.5f, 0.f)};
+  sch_face face_neg_z = {0};
+  sch_create_face(4, vertices_neg_z, &face_neg_z);
+
+  simd4f vertices_pos_x[4] = {
+      simd4f_create(0.5f, -0.5f, 0.5f, 0.f),
+      simd4f_create(0.5f, -0.5f, -0.5f, 0.f),
+      simd4f_create(0.5f, 0.5f, -0.5f, 0.f),
+      simd4f_create(0.5f, 0.5f, 0.5f, 0.f)};
+
+  sch_face face_pos_x = {0};
+  sch_create_face(4, vertices_pos_x, &face_pos_x);
+
+  simd4f vertices_neg_x[4] = {
+      simd4f_create(-0.5f, -0.5f, 0.5f, 0.f),
+      simd4f_create(-0.5f, -0.5f, -0.5f, 0.f),
+      simd4f_create(-0.5f, 0.5f, -0.5f, 0.f),
+      simd4f_create(-0.5f, 0.5f, 0.5f, 0.f)};
+
+  sch_face face_neg_x = {0};
+  sch_create_face(4, vertices_neg_x, &face_neg_x);
+}
+
 #endif /* SCONVCOL_IMPLEMENTATION */
 
 #ifdef __cplusplus

tests/sconvcolTests.cc

@@ -71,11 +71,11 @@ TEST_CASE("HullBuilder", "[sconvcol]") {
     sch_builder_reset(&builder);
 
     WHEN("adding two vertices ") {
-      simd4f vertex0 = simd4f_create(1.f, 0.f, 0.f, 1.f);
+      simd4f vertex0 = simd4f_create(0.f, 0.f, 0.f, 1.f);
       sch_builder_face_begin(&builder);
       sch_builder_face_vertex(&builder, vertex0);
 
-      simd4f vertex1 = simd4f_create(0.f, 0.f, 0.f, 1.f);
+      simd4f vertex1 = simd4f_create(1.f, 0.f, 0.f, 1.f);
       sch_builder_face_vertex(&builder, vertex1);
 
       WHEN("ending the face") {
@@ -102,9 +102,10 @@ TEST_CASE("HullBuilder", "[sconvcol]") {
 
         WHEN("creating a hull") {
           sch_hull hull;
-          sch_builder_create_hull(&builder, &hull);
+          SchHullResult result = sch_builder_create_hull(&builder, &hull);
 
           THEN("verify triangle hull") {
+            REQUIRE(result == SchHullResultOK);
             REQUIRE(hull.num_faces == 1);
             sch_face* face = &hull.faces[0];
 
@@ -118,43 +119,39 @@ TEST_CASE("HullBuilder", "[sconvcol]") {
 
             REQUIRE(face->edge->next->next->next == face->edge);
           }
+
+          sch_hull_free_memory(&hull);
+        }
+
+        WHEN("adding concave face") {
+          sch_hull hull;
+          SchHullResult result = sch_builder_create_hull(&builder, &hull);
+
+          simd4f vertex3 = simd4f_create(0.5f, 0.25f, 0.1337f, 1.f);
+
+          sch_builder_face_begin(&builder);
+          sch_builder_face_vertex(&builder, vertex0);
+          sch_builder_face_vertex(&builder, vertex1);
+          sch_builder_face_vertex(&builder, vertex3);
+          sch_builder_face_end(&builder);
+
+          THEN("check face and vertex definitions") {
+            REQUIRE(builder.num_faces == 2);
+            REQUIRE(builder.num_vertices == 4);
+            REQUIRE(builder.face_vert_idx_start[0] == 0);
+            REQUIRE(builder.face_vert_idx_end[0] == 2);
+            REQUIRE(builder.face_vert_idx_start[1] == 3);
+            REQUIRE(builder.face_vert_idx_end[1] == 5);
+          }
+
+          THEN("hull creation fails") {
+            SchHullResult result = sch_builder_create_hull(&builder, &hull);
+            REQUIRE(result == SchHullResultConcaveVertex);
+          }
+
+          sch_hull_free_memory(&hull);
         }
       }
     }
   }
 }
-
-TEST_CASE("Hull Get Plane", "[sconvcol]") {
-  simd4f vertices_pos_z[4] = {
-      simd4f_create(-1.f, -1.f, 1.f, 0.f),
-      simd4f_create(1.f, -1.f, 1.f, 0.f),
-      simd4f_create(1.f, 1.f, 1.f, 0.f),
-      simd4f_create(-1.f, 1.f, 1.f, 0.f)};
-
-  sch_face face_pos_z = {nullptr};
-  sch_create_face(4, vertices_pos_z, &face_pos_z);
-
-  simd4f vertices_pos_x[4] = {
-      simd4f_create(1.f, -1.f, 1.f, 0.f),
-      simd4f_create(1.f, -1.f, -1.f, 0.f),
-      simd4f_create(1.f, 1.f, -1.f, 0.f),
-      simd4f_create(1.f, 1.f, 1.f, 0.f)};
-
-  sch_face face_pos_x = {nullptr};
-  sch_create_face(4, vertices_pos_x, &face_pos_x);
-
-  sch_hull hull = {0};
-  sch_face faces[2] = {face_pos_z, face_pos_x};
-  sch_create_hull(2, faces, &hull);
-
-  sch_plane plane_pos_z;
-  sch_hull_get_plane(&hull, 0, &plane_pos_z);
-
-  simd4f point = simd4f_create(0.f, 0.f, 1.23f, 0.f);
-
-  float dist = sch_plane_distance(&plane_pos_z, &point);
-
-  REQUIRE_THAT(dist, Catch::Matchers::WithinRel(0.23, 1.0e-3));
-}
-
-TEST_CASE("Hull Get Support", "[sconvcol]") {}