diff --git a/src/sconvcol.h b/src/sconvcol.h index 2dadde3..c3f8d91 100644 --- a/src/sconvcol.h +++ b/src/sconvcol.h @@ -5,8 +5,8 @@ extern "C" { #endif -#include #include +#include #include "vectorial/simd4x4f.h" @@ -15,113 +15,255 @@ 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; - sch_edge* twin; - sch_face* face; - sch_edge* next; + sch_vert* vert; + sch_edge* twin; + sch_face* face; + sch_edge* next; }; struct sch_vert { - simd4f p; - sch_edge* edge; + simd4f p; + sch_edge* edge; }; struct sch_face { - sch_edge* edge; + sch_edge* edge; }; struct sch_hull { - int num_faces; - sch_face* faces; + int num_faces; + sch_face* faces; + sch_edge* edges; + sch_vert* vertices; }; struct sch_plane { - simd4f p; - simd4f n; + simd4f p; + simd4f n; }; -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_support (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, - sch_hull* out_hull); +struct sch_face_query { + sch_hull* hull_A; + sch_hull* hull_B; + float dist; + int face_idx_A; + int face_idx_B; +}; // -// srender Implementation +// 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(const sch_hull* hull, int index, sch_plane* out_plane); + +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, sch_hull* out_hull); + +void sch_query_face_dir( + const sch_hull* hull_A, + const sch_hull* hull_B, + sch_face_query* out_query); + +// +// sconvcol Implementation // #ifdef SCONVCOL_IMPLEMENTATION -float sch_plane_distance (sch_plane* plane, simd4f* v) { - return simd4f_dot3_scalar(simd4f_sub (*v, plane->p), plane->n); +float sch_plane_distance(sch_plane* plane, simd4f* v) { + return simd4f_dot3_scalar(simd4f_sub(*v, plane->p), plane->n); } -void sch_create_face (int num_vert, simd4f* vertices, sch_face* out_face) { - assert(out_face != NULL); - assert(out_face->edge == NULL); +void sch_create_face(int num_vert, simd4f* vertices, sch_face* out_face) { + assert(out_face != NULL); + assert(out_face->edge == NULL); - int i = 0; - sch_edge* f_edges = malloc(sizeof(sch_edge) * num_vert); - sch_vert* f_verts = malloc(sizeof(sch_vert) * num_vert); + int i = 0; + sch_edge* f_edges = malloc(sizeof(sch_edge) * num_vert); + sch_vert* f_verts = malloc(sizeof(sch_vert) * num_vert); - while (i < num_vert) { - sch_vert* vert = &f_verts[i]; - sch_edge* edge = &f_edges[i]; + while (i < num_vert) { + sch_vert* vert = &f_verts[i]; + sch_edge* edge = &f_edges[i]; - edge->twin = NULL; - edge->vert = vert; - edge->face = out_face; - edge->next = &f_edges[(i + 1) % num_vert]; + edge->twin = NULL; + edge->vert = vert; + edge->face = out_face; + edge->next = &f_edges[(i + 1) % num_vert]; - vert->edge = edge; - vert->p = vertices[i]; + vert->edge = edge; + vert->p = vertices[i]; - i++; - } + i++; + } - out_face->edge = &f_edges[0]; + out_face->edge = &f_edges[0]; } -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)); - *out_dir = simd4f_mul (*out_dir, simd4f_splat(recip_len)); +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.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, - 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_builder_reset(sch_hull_builder* builder) { + builder->num_vertices = 0; + builder->num_faces = 0; } -void sch_hull_get_plane (sch_hull* hull, int index, sch_plane* out_plane) { - assert (hull != NULL); - assert (index >= 0 && index < hull->num_faces); - assert (out_plane != NULL); +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; +} - // 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; - simd4f dir1; - sch_edge_get_dir(edge0, &dir0); - sch_edge_get_dir(edge1, &dir1); +void sch_builder_face_end(sch_hull_builder* builder) { builder->num_faces++; } - out_plane->p = edge0->vert->p; - out_plane->n = simd4f_cross3 (dir0, dir1); +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); + + 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); + 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; + simd4f dir1; + sch_edge_get_dir(edge0, &dir0); + sch_edge_get_dir(edge1, &dir1); + + out_plane->p = edge0->vert->p; + out_plane->n = simd4f_cross3(dir0, dir1); } #endif /* SCONVCOL_IMPLEMENTATION */ diff --git a/tests/sconvcolTests.cc b/tests/sconvcolTests.cc index 115138f..9dfd785 100644 --- a/tests/sconvcolTests.cc +++ b/tests/sconvcolTests.cc @@ -3,68 +3,158 @@ #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); + simd4f point = simd4f_create(0.f, 0.23f, 0.f, 0.f); - float dist = sch_plane_distance (&plane, &point); + float dist = sch_plane_distance(&plane, &point); - REQUIRE (dist == 0.23f); + REQUIRE(dist == 0.23f); } TEST_CASE("Edge Get Direction", "[sconvcol]") { - simd4f vertices[4] = { - 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 vertices[4] = { + 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_create_face (4, vertices, &face); + sch_face face = {nullptr}; + sch_create_face(4, vertices, &face); - simd4f dir; - sch_edge_get_dir (face.edge, &dir); + simd4f dir; + sch_edge_get_dir(face.edge, &dir); - simd4f dir_ref = simd4f_create (1.f, 0.f, 0.f, 0.f); - REQUIRE (1.f == simd4f_dot3_scalar (dir, dir_ref)); + simd4f dir_ref = simd4f_create(1.f, 0.f, 0.f, 0.f); + 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 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 = {0}; - sch_create_face (4, vertices_pos_z, &face_pos_z); + 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 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 = {0}; - sch_create_face (4, vertices_pos_x, &face_pos_x); + 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_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); + 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); + simd4f point = simd4f_create(0.f, 0.f, 1.23f, 0.f); - float dist = sch_plane_distance (&plane_pos_z, &point); + float dist = sch_plane_distance(&plane_pos_z, &point); - REQUIRE_THAT(dist, Catch::Matchers::WithinRel(0.23, 1.0e-3)); + REQUIRE_THAT(dist, Catch::Matchers::WithinRel(0.23, 1.0e-3)); } +TEST_CASE("Hull Get Support", "[sconvcol]") {}