martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Check vor concave vertices when building convex hull objects

master
Martin Felis 2021-05-27 14:18:21 +02:00
parent 82de09ad8b
commit d8a67db2a9
2 changed files with 124 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
src/sconvcol.h
View File

@ -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) {
out_hull->num_faces = builder->num_faces;
void sch_builder_allocate_memory (sch_hull_builder* builder, sch_hull* out_hull) {
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

73
tests/sconvcolTests.cc
View File

@ -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]") {}