Initial work on a convex hull builder object.

master
Martin Felis 2021-05-26 11:33:54 +02:00
parent 0081cff8bc
commit 9356ec14f8
2 changed files with 342 additions and 110 deletions

View File

@ -5,8 +5,8 @@
extern "C" {
#endif
#include <stdbool.h>
#include <assert.h>
#include <stdbool.h>
#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 */

View File

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