Initial work on a convex hull builder object.

src/sconvcol.h

@@ -5,8 +5,8 @@
 extern "C" {
 #endif
 
-#include <stdbool.h>
 #include <assert.h>
+#include <stdbool.h>
 
 #include "vectorial/simd4x4f.h"
 
@@ -15,6 +15,8 @@ typedef struct sch_vert sch_vert;
 typedef struct sch_face sch_face;
 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;
 
 struct sch_edge {
   sch_vert* vert;
@@ -35,6 +37,8 @@ struct sch_face {
 struct sch_hull {
   int num_faces;
   sch_face* faces;
+  sch_edge* edges;
+  sch_vert* vertices;
 };
 
 struct sch_plane {
@@ -42,21 +46,64 @@ struct sch_plane {
   simd4f n;
 };
 
+struct sch_face_query {
+  sch_hull* hull_A;
+  sch_hull* hull_B;
+  float dist;
+  int face_idx_A;
+  int face_idx_B;
+};
+
+//
+// Hull Builder
+//
+
+#define SCH_BUILDER_MAX_NUM_VERTICES 1024
+#define SCH_BUILDER_MAX_NUM_FACES 256
+#define SCH_BUILDER_MAX_NUM_FACE_VERTICES 32
+
+struct sch_hull_builder {
+  int num_vertices;
+  int num_faces;
+  simd4f vertices[SCH_BUILDER_MAX_NUM_VERTICES];
+  int face_vert_idx[SCH_BUILDER_MAX_NUM_FACES * SCH_BUILDER_MAX_NUM_VERTICES];
+  int face_vert_idx_start[SCH_BUILDER_MAX_NUM_FACES];
+  int face_vert_idx_end[SCH_BUILDER_MAX_NUM_FACES];
+};
+
+void sch_builder_reset(sch_hull_builder* builder);
+
+void sch_builder_face_begin(sch_hull_builder* builder);
+
+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);
+
+//
+// Calculations
+//
+
 float sch_plane_distance(sch_plane* plane, simd4f* v);
 
 void sch_edge_get_dir(sch_edge* edge, simd4f* dir);
 
-void sch_hull_get_plane (sch_hull* hull, int index, sch_plane *out_plane);
+void sch_hull_get_plane(const sch_hull* hull, int index, sch_plane* out_plane);
 
-void sch_hull_get_support (sch_hull* hull, simd4f n, simd4f* out_vert);
+void sch_hull_get_support(const sch_hull* hull, simd4f n, simd4f* out_vert);
 
 void sch_create_face(int num_vert, simd4f* vertices, sch_face* out_face);
 
-void sch_create_hull (int num_faces, sch_face* faces,
+void sch_create_hull(int num_faces, sch_face* faces, sch_hull* out_hull);
-		sch_hull* out_hull);
+
+void sch_query_face_dir(
+    const sch_hull* hull_A,
+    const sch_hull* hull_B,
+    sch_face_query* out_query);
 
 //
-// srender Implementation
+// sconvcol Implementation
 //
 
 #ifdef SCONVCOL_IMPLEMENTATION
@@ -93,20 +140,115 @@ void sch_create_face (int num_vert, simd4f* vertices, sch_face* out_face) {
 
 void sch_edge_get_dir(sch_edge* edge, simd4f* out_dir) {
   *out_dir = simd4f_sub(edge->next->vert->p, edge->vert->p);
-	float recip_len = 1. / sqrt(simd4f_dot3_scalar (*out_dir, *out_dir));
+  float recip_len = 1.f / sqrtf(simd4f_dot3_scalar(*out_dir, *out_dir));
   *out_dir = simd4f_mul(*out_dir, simd4f_splat(recip_len));
 }
 
-void sch_create_hull (int num_faces, sch_face* faces,
+void sch_builder_reset(sch_hull_builder* builder) {
-		sch_hull* out_hull) {
+  builder->num_vertices = 0;
+  builder->num_faces = 0;
+}
+
+void sch_builder_face_begin(sch_hull_builder* builder) {
+  int face_index = builder->num_faces;
+  builder->face_vert_idx_start[face_index] = builder->num_vertices;
+  builder->face_vert_idx_end[face_index] = -1;
+}
+
+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) {
+  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++) {
+    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;
+
+    num_edges += edge_count;
+  }
+
+  out_hull->edges = (sch_edge*)malloc(sizeof(sch_edge) * num_edges);
+  int hull_edge_idx = 0;
+
+  for (int face_index = 0; face_index < out_hull->num_faces; face_index++) {
+    sch_face* face = &out_hull->faces[face_index];
+
+    int start_idx = builder->face_vert_idx_start[face_index];
+    int end_idx = builder->face_vert_idx_end[face_index];
+
+    sch_edge* prev_edge = NULL;
+
+    for (int face_vert_idx = start_idx; face_vert_idx <= end_idx;
+         face_vert_idx++) {
+      sch_edge* edge = &out_hull->edges[hull_edge_idx];
+      hull_edge_idx++;
+      if (face_vert_idx == start_idx) {
+        face->edge = edge;
+      }
+
+      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 (face_vert_idx == end_idx) {
+        edge->next = face->edge;
+      }
+      if (prev_edge != NULL) {
+        prev_edge->next = edge;
+      }
+
+      prev_edge = edge;
+    }
+  }
+}
+
+void sch_builder_face_vertex(sch_hull_builder* builder, simd4f vertex) {
+  int face_index = builder->num_faces;
+  int vert_index = -1;
+  for (int i = 0; i < builder->num_vertices; i++) {
+    if (simd4f_get_x(
+            simd4f_length4_squared(simd4f_sub(builder->vertices[i], vertex)))
+        < 1.0e-4) {
+      vert_index = i;
+      break;
+    }
+  }
+
+  if (vert_index == -1) {
+    vert_index = builder->num_vertices;
+    builder->vertices[vert_index] = vertex;
+    builder->num_vertices++;
+  }
+
+  if (builder->face_vert_idx_end[face_index] == -1) {
+    builder->face_vert_idx_end[face_index] =
+        builder->face_vert_idx_start[face_index];
+  } else {
+    builder->face_vert_idx_end[face_index]++;
+  }
+
+  int face_end_idx = builder->face_vert_idx_end[face_index];
+  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);
-	assert (out_hull->num_faces == NULL);
 
   out_hull->num_faces = num_faces;
   out_hull->faces = faces;
 }
 
-void sch_hull_get_plane (sch_hull* hull, int index, sch_plane* out_plane) {
+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);
   assert(out_plane != NULL);

tests/sconvcolTests.cc

@@ -3,10 +3,16 @@
 #include "catch.hpp"
 #include "sconvcol.h"
 
+bool simd4f_equal(simd4f a, simd4f b) {
+  return (simd4f_get_x(simd4f_length4_squared(simd4f_sub(a, b))) == 0.f);
+}
+
 using namespace std;
 
 TEST_CASE("Plane Distance", "[sconvcol]") {
-	sch_plane plane = {simd4f_create (0.f, 0.f, 0.f, 0.f), simd4f_create(0.f, 1.f, 0.f, 0.f)};
+  sch_plane plane = {
+      simd4f_create(0.f, 0.f, 0.f, 0.f),
+      simd4f_create(0.f, 1.f, 0.f, 0.f)};
 
   simd4f point = simd4f_create(0.f, 0.23f, 0.f, 0.f);
 
@@ -20,10 +26,9 @@ TEST_CASE("Edge Get Direction", "[sconvcol]") {
       simd4f_create(-1.f, -1.f, 0.f, 0.f),
       simd4f_create(1.f, -1.f, 0.f, 0.f),
       simd4f_create(1.f, 1.f, 0.f, 0.f),
-		simd4f_create (-1.f,  1.f, 0.f, 0.f)
+      simd4f_create(-1.f, 1.f, 0.f, 0.f)};
-	};
 
-	sch_face face = {0};
+  sch_face face = {nullptr};
   sch_create_face(4, vertices, &face);
 
   simd4f dir;
@@ -33,25 +38,109 @@ 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;
+
+  builder.num_vertices = 3;
+
+  GIVEN("A builder") {
+    WHEN("is reset") {
+      sch_builder_reset(&builder);
+
+      THEN("number of vertices and faces is set to 0") {
+        REQUIRE(builder.num_vertices == 0);
+        REQUIRE(builder.num_faces == 0);
+      }
+
+      WHEN("adding a vertex") {
+        int num_vertices = builder.num_vertices;
+
+        simd4f vertex = simd4f_create(1.f, 0.f, 0.f, 1.f);
+        sch_builder_face_begin(&builder);
+        sch_builder_face_vertex(&builder, vertex);
+        THEN("number of vertices increased") {
+          REQUIRE(builder.num_vertices > num_vertices);
+        }
+      }
+    }
+  }
+
+  GIVEN("An empty builder") {
+    sch_builder_reset(&builder);
+
+    WHEN("adding two vertices ") {
+      simd4f vertex0 = simd4f_create(1.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);
+      sch_builder_face_vertex(&builder, vertex1);
+
+      WHEN("ending the face") {
+        sch_builder_face_end(&builder);
+        THEN("face idx range goes from 0 to 1") {
+          REQUIRE(builder.num_faces == 1);
+          REQUIRE(builder.num_vertices == 2);
+          REQUIRE(builder.face_vert_idx_start[0] == 0);
+          REQUIRE(builder.face_vert_idx_end[0] == 1);
+        }
+      }
+
+      WHEN("creating a triangle") {
+        simd4f vertex2 = simd4f_create(0.5f, 1.f, 0.f, 1.f);
+        sch_builder_face_vertex(&builder, vertex2);
+        sch_builder_face_end(&builder);
+
+        THEN("face idx range goes from 0 to 2") {
+          REQUIRE(builder.num_faces == 1);
+          REQUIRE(builder.num_vertices == 3);
+          REQUIRE(builder.face_vert_idx_start[0] == 0);
+          REQUIRE(builder.face_vert_idx_end[0] == 2);
+        }
+
+        WHEN("creating a hull") {
+          sch_hull hull;
+          sch_builder_create_hull(&builder, &hull);
+
+          THEN("verify triangle hull") {
+            REQUIRE(hull.num_faces == 1);
+            sch_face* face = &hull.faces[0];
+
+            sch_vert* hull_vert0 = face->edge->vert;
+            sch_vert* hull_vert1 = face->edge->next->vert;
+            sch_vert* hull_vert2 = face->edge->next->next->vert;
+
+            REQUIRE(simd4f_equal(hull_vert0->p, vertex0));
+            REQUIRE(simd4f_equal(hull_vert1->p, vertex1));
+            REQUIRE(simd4f_equal(hull_vert2->p, vertex2));
+
+            REQUIRE(face->edge->next->next->next == face->edge);
+          }
+        }
+      }
+    }
+  }
+}
+
 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)
+      simd4f_create(-1.f, 1.f, 1.f, 0.f)};
-	};
 
-	sch_face face_pos_z = {0};
+  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)
+      simd4f_create(1.f, 1.f, 1.f, 0.f)};
-	};
 
-	sch_face face_pos_x = {0};
+  sch_face face_pos_x = {nullptr};
   sch_create_face(4, vertices_pos_x, &face_pos_x);
 
   sch_hull hull = {0};
@@ -68,3 +157,4 @@ TEST_CASE("Hull Get Plane", "[sconvcol]") {
   REQUIRE_THAT(dist, Catch::Matchers::WithinRel(0.23, 1.0e-3));
 }
 
+TEST_CASE("Hull Get Support", "[sconvcol]") {}