417 lines
11 KiB
C
417 lines
11 KiB
C
#ifndef SCONVCOL_H
|
|
#define SCONVCOL_H
|
|
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
|
|
#include <assert.h>
|
|
#include <stdbool.h>
|
|
|
|
#include "vectorial/simd4x4f.h"
|
|
|
|
inline bool sch_simd4f_equal(simd4f a, simd4f b) {
|
|
return (simd4f_get_x(simd4f_length4_squared(simd4f_sub(a, b))) == 0.f);
|
|
}
|
|
|
|
typedef struct sch_edge sch_edge;
|
|
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;
|
|
};
|
|
|
|
struct sch_vert {
|
|
simd4f p;
|
|
sch_edge* edge;
|
|
};
|
|
|
|
struct sch_face {
|
|
sch_edge* edge;
|
|
};
|
|
|
|
struct sch_hull {
|
|
sch_face* faces;
|
|
sch_edge* edges;
|
|
sch_vert* vertices;
|
|
int num_faces;
|
|
int num_edges;
|
|
int num_vertices;
|
|
};
|
|
|
|
struct sch_plane {
|
|
simd4f p;
|
|
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
|
|
|
|
enum SchHullResult {
|
|
SchHullResultOK = 0,
|
|
SchHullResultConcaveVertex = -1,
|
|
SchHullResultWrongWinding = -2,
|
|
SchHullResultInvalidTwinEdges = -3,
|
|
SchHullResultOpenHull = -4
|
|
};
|
|
|
|
typedef enum SchHullResult SchHullResult;
|
|
|
|
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);
|
|
|
|
SchHullResult sch_builder_create_hull(sch_hull_builder* builder, sch_hull* out_hull);
|
|
|
|
//
|
|
// Calculations
|
|
//
|
|
|
|
float sch_plane_distance(const sch_plane* plane, const simd4f* v);
|
|
|
|
void sch_edge_get_dir(const sch_edge* edge, simd4f* out_dir);
|
|
|
|
void sch_hull_free_memory (sch_hull* hull);
|
|
|
|
void sch_hull_get_plane(const sch_hull* hull, const int index, sch_plane* out_plane);
|
|
|
|
sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4f v1);
|
|
|
|
void sch_hull_get_support(const sch_hull* hull, simd4f n, simd4f* out_vert);
|
|
|
|
int sch_hull_is_vertex_concave(const sch_hull* hull, const simd4f p);
|
|
|
|
int sch_hull_is_closed (const sch_hull* hull);
|
|
|
|
int sch_hull_connect_face_edges(const sch_hull* hull, int face_index);
|
|
|
|
void sch_create_face(int num_vert, simd4f* vertices, sch_face* out_face);
|
|
|
|
//
|
|
// sconvcol Implementation
|
|
//
|
|
|
|
#ifdef SCONVCOL_IMPLEMENTATION
|
|
|
|
float sch_plane_distance(const sch_plane* plane, const 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);
|
|
|
|
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];
|
|
|
|
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];
|
|
|
|
i++;
|
|
}
|
|
|
|
out_face->edge = &f_edges[0];
|
|
}
|
|
|
|
void sch_edge_get_dir(const 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_builder_reset(sch_hull_builder* builder) {
|
|
builder->num_vertices = 0;
|
|
builder->num_faces = 0;
|
|
}
|
|
|
|
void sch_builder_face_begin(sch_hull_builder* builder) {
|
|
int face_index = builder->num_faces;
|
|
if (builder->num_faces == 0) {
|
|
builder->face_vert_idx_start[face_index] = 0;
|
|
} else {
|
|
builder->face_vert_idx_start[face_index] = builder->face_vert_idx_end[face_index - 1] + 1;
|
|
}
|
|
|
|
builder->face_vert_idx_end[face_index] = -1;
|
|
}
|
|
|
|
void sch_builder_face_end(sch_hull_builder* builder) { 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->num_vertices = builder->num_vertices;
|
|
out_hull->vertices =
|
|
(sch_vert*)malloc(sizeof(sch_vert) * builder->num_vertices);
|
|
|
|
int num_edges = 0;
|
|
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;
|
|
|
|
num_edges += edge_count + 1;
|
|
}
|
|
|
|
out_hull->num_edges = num_edges;
|
|
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;
|
|
hull->num_faces = 0;
|
|
free (hull->vertices);
|
|
hull->vertices = NULL;
|
|
hull->num_vertices = 0;
|
|
free (hull->edges);
|
|
hull->edges = NULL;
|
|
hull->num_edges = 0;
|
|
}
|
|
|
|
SchHullResult sch_builder_create_hull(sch_hull_builder* builder, sch_hull* out_hull) {
|
|
sch_builder_allocate_memory(builder, out_hull);
|
|
out_hull->num_faces = 0;
|
|
|
|
int hull_edge_idx = 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];
|
|
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 (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;
|
|
}
|
|
if (prev_edge != NULL) {
|
|
prev_edge->next = edge;
|
|
}
|
|
|
|
prev_edge = edge;
|
|
}
|
|
|
|
int edge_add_result = sch_hull_connect_face_edges (out_hull, face_index);
|
|
if (edge_add_result != SchHullResultOK) {
|
|
sch_hull_free_memory(out_hull);
|
|
return edge_add_result;
|
|
}
|
|
|
|
out_hull->num_faces = face_index + 1;
|
|
}
|
|
|
|
assert (hull_edge_idx == out_hull->num_edges);
|
|
|
|
return sch_hull_is_closed (out_hull);
|
|
}
|
|
|
|
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_hull_get_plane(const sch_hull* hull, const 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);
|
|
}
|
|
|
|
sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4f v1) {
|
|
for (int fi = 0; fi < hull->num_faces; fi++) {
|
|
sch_face* face = &hull->faces[fi];
|
|
sch_edge* edge0 = face->edge;
|
|
sch_edge* edge = edge0;
|
|
do {
|
|
if (sch_simd4f_equal(edge->vert->p, v0)
|
|
&& sch_simd4f_equal(edge->next->vert->p, v1)) {
|
|
return edge;
|
|
}
|
|
edge = edge->next;
|
|
} while (edge != edge0);
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
int sch_hull_is_vertex_concave(const sch_hull* hull, const 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;
|
|
}
|
|
|
|
int sch_hull_connect_face_edges(const sch_hull* hull, int new_face_index) {
|
|
sch_face* new_face = &hull->faces[new_face_index];
|
|
|
|
sch_edge* new_face_edge0 = new_face->edge;
|
|
sch_edge* new_face_edge = new_face_edge0;
|
|
|
|
do {
|
|
sch_vert* new_edge_v0 = new_face_edge->vert;
|
|
sch_vert* new_edge_v1 = new_face_edge->next->vert;
|
|
|
|
for (int fi = 0; fi < hull->num_faces; fi++) {
|
|
if (fi == new_face_index) {
|
|
continue;
|
|
}
|
|
|
|
sch_face* face = &hull->faces[fi];
|
|
sch_edge* face_edge0 = face->edge;
|
|
sch_edge* face_edge = face_edge0;
|
|
|
|
do {
|
|
sch_vert* edge_v0 = face_edge->vert;
|
|
sch_vert* edge_v1 = face_edge->next->vert;
|
|
|
|
if (sch_simd4f_equal(new_edge_v0->p, edge_v0->p)
|
|
&& sch_simd4f_equal(new_edge_v1->p, edge_v1->p)) {
|
|
return SchHullResultWrongWinding;
|
|
} else if (sch_simd4f_equal(new_edge_v0->p, edge_v1->p)
|
|
&& sch_simd4f_equal(new_edge_v1->p, edge_v0->p)) {
|
|
if (new_face_edge->twin == NULL && face_edge->twin == NULL) {
|
|
new_face_edge->twin = face_edge;
|
|
face_edge->twin = new_face_edge;
|
|
} else if (new_face_edge->twin == face_edge->twin) {
|
|
// nothing to do
|
|
} else if ((new_face_edge->twin == NULL && face_edge->twin != NULL)
|
|
|| (new_face_edge->twin != NULL && face_edge->twin == NULL)) {
|
|
return SchHullResultInvalidTwinEdges;
|
|
}
|
|
}
|
|
|
|
face_edge = face_edge->next;
|
|
} while (face_edge != face_edge0);
|
|
}
|
|
|
|
new_face_edge = new_face_edge->next;
|
|
} while (new_face_edge != new_face_edge0);
|
|
|
|
return SchHullResultOK;
|
|
}
|
|
|
|
int sch_hull_is_closed (const sch_hull* hull) {
|
|
for (int ei = 0; ei < hull->num_edges; ei++) {
|
|
if (hull->edges[ei].twin == NULL) {
|
|
return SchHullResultOpenHull;
|
|
}
|
|
}
|
|
|
|
return SchHullResultOK;
|
|
}
|
|
|
|
#endif /* SCONVCOL_IMPLEMENTATION */
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|
|
|
|
#endif /* SCONVCOL_H */
|