X-Git-Url: http://git.hungrycats.org/cgi-bin/gitweb.cgi?p=xscreensaver;a=blobdiff_plain;f=hacks%2Fglx%2Fquickhull.c;fp=hacks%2Fglx%2Fquickhull.c;h=bf3036a1539bd61ded1953317c36c3227adafa0b;hp=0000000000000000000000000000000000000000;hb=78add6e627ee5f10e1fa6f3852602ea5066eee5a;hpb=39809ded547bdbb08207d3e514950425215b4410 diff --git a/hacks/glx/quickhull.c b/hacks/glx/quickhull.c new file mode 100644 index 00000000..bf3036a1 --- /dev/null +++ b/hacks/glx/quickhull.c @@ -0,0 +1,1348 @@ +/* quickhull, Copyright (c) 2016 Karim Naaji, karim.naaji@gmail.com + https://github.com/karimnaaji/3d-quickhull + + LICENCE: + The MIT License (MIT) + + Copyright (c) 2016 Karim Naaji, karim.naaji@gmail.com + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE + + REFERENCES: + [1] http://box2d.org/files/GDC2014/DirkGregorius_ImplementingQuickHull.pdf + [2] http://www.cs.smith.edu/~orourke/books/compgeom.html + [3] http://www.flipcode.com/archives/The_Half-Edge_Data_Structure.shtml + [4] http://doc.cgal.org/latest/HalfedgeDS/index.html + [5] http://thomasdiewald.com/blog/?p=1888 + [6] https://fgiesen.wordpress.com/2012/02/21/half-edge-based-mesh-representations-theory/ + + HOWTO: + #define QUICKHULL_DEBUG // Only if assertions need to be checked + #include "quickhull.h" + + HISTORY: + - 25-Feb-2018: jwz: adapted for xscreensaver + - 1.0.1 (2016-11-01): Various improvements over epsilon issues and + degenerate faces + Debug functionalities to test final results dynamically + API to export hull meshes in OBJ files + - 1.0 (2016-09-10): Initial + + TODO: + - use float* from public interface + - reduce memory usage +*/ + +#ifdef HAVE_CONFIG_H +# include "config.h" +#endif /* HAVE_CONFIG_H */ + +#include "quickhull.h" + +#include /* sqrt & fabs */ +#include /* FILE */ +#include /* memcpy */ + +/* Quickhull helpers, define your own if needed */ +#ifndef QUICKHULL_HELPERS +#include /* malloc, free, realloc */ +#define QUICKHULL_HELPERS 1 +#define QH_MALLOC(T, N) ((T*) malloc(N * sizeof(T))) +#define QH_REALLOC(T, P, N) ((T*)realloc(P, sizeof(T) * N)) +#define QH_FREE(T) free(T) +#define QH_SWAP(T, A, B) { T tmp = B; B = A; A = tmp; } +#ifdef QUICKHULL_DEBUG +#define QH_ASSERT(STMT) if (!(STMT)) { *(int *)0 = 0; } +#define QH_LOG(FMT, ...) printf(FMT, ## __VA_ARGS__) +#else +#define QH_ASSERT(STMT) +#define QH_LOG(FMT, ...) +#endif /* QUICKHULL_DEBUG */ +#endif /* QUICKHULL_HELPERS */ + +#ifndef QH_FLT_MAX +#define QH_FLT_MAX 1e+37F +#endif + +#ifndef QH_FLT_EPS +#define QH_FLT_EPS 1E-5F +#endif + +#ifndef QH_VERTEX_SET_SIZE +#define QH_VERTEX_SET_SIZE 128 +#endif + +typedef long qh_index_t; + +typedef struct qh_half_edge { + qh_index_t opposite_he; /* index of the opposite half edge */ + qh_index_t next_he; /* index of the next half edge */ + qh_index_t previous_he; /* index of the previous half edge */ + qh_index_t he; /* index of the current half edge */ + qh_index_t to_vertex; /* index of the next vertex */ + qh_index_t adjacent_face; /* index of the ajacent face */ +} qh_half_edge_t; + +typedef struct qh_index_set { + qh_index_t* indices; + unsigned int size; + unsigned int capacity; +} qh_index_set_t; + +typedef struct qh_face { + qh_index_set_t iset; + qh_vec3_t normal; + qh_vertex_t centroid; + qh_index_t edges[3]; + qh_index_t face; + double sdist; + int visitededges; +} qh_face_t; + +typedef struct qh_index_stack { + qh_index_t* begin; + unsigned int size; +} qh_index_stack_t; + +typedef struct qh_context { + qh_face_t* faces; + qh_half_edge_t* edges; + qh_vertex_t* vertices; + qh_vertex_t centroid; + qh_index_stack_t facestack; + qh_index_stack_t scratch; + qh_index_stack_t horizonedges; + qh_index_stack_t newhorizonedges; + char* valid; + unsigned int nedges; + unsigned int nvertices; + unsigned int nfaces; + + #ifdef QUICKHULL_DEBUG + unsigned int maxfaces; + unsigned int maxedges; + #endif +} qh_context_t; + +static void +qh__find_6eps(qh_vertex_t* vertices, unsigned int nvertices, qh_index_t* eps) +{ + qh_vertex_t* ptr = vertices; + + double minxy = +QH_FLT_MAX; + double minxz = +QH_FLT_MAX; + double minyz = +QH_FLT_MAX; + + double maxxy = -QH_FLT_MAX; + double maxxz = -QH_FLT_MAX; + double maxyz = -QH_FLT_MAX; + + int i = 0; + for (i = 0; i < 6; ++i) { + eps[i] = 0; + } + + for (i = 0; i < nvertices; ++i) { + if (ptr->z < minxy) { + eps[0] = i; + minxy = ptr->z; + } + if (ptr->y < minxz) { + eps[1] = i; + minxz = ptr->y; + } + if (ptr->x < minyz) { + eps[2] = i; + minyz = ptr->x; + } + if (ptr->z > maxxy) { + eps[3] = i; + maxxy = ptr->z; + } + if (ptr->y > maxxz) { + eps[4] = i; + maxxz = ptr->y; + } + if (ptr->x > maxyz) { + eps[5] = i; + maxyz = ptr->x; + } + ptr++; + } +} + +static double +qh__vertex_segment_length2(qh_vertex_t* p, qh_vertex_t* a, qh_vertex_t* b) +{ + double dx = b->x - a->x; + double dy = b->y - a->y; + double dz = b->z - a->z; + + double d = dx * dx + dy * dy + dz * dz; + + double x = a->x; + double y = a->y; + double z = a->z; + + if (d != 0) { + double t = ((p->x - a->x) * dx + + (p->y - a->y) * dy + + (p->z - a->z) * dz) / d; + + if (t > 1) { + x = b->x; + y = b->y; + z = b->z; + } else if (t > 0) { + x += dx * t; + y += dy * t; + z += dz * t; + } + } + + dx = p->x - x; + dy = p->y - y; + dz = p->z - z; + + return dx * dx + dy * dy + dz * dz; +} + +static void +qh__vec3_sub(qh_vec3_t* a, qh_vec3_t* b) +{ + a->x -= b->x; + a->y -= b->y; + a->z -= b->z; +} + +static void +qh__vec3_add(qh_vec3_t* a, qh_vec3_t* b) +{ + a->x += b->x; + a->y += b->y; + a->z += b->z; +} + +static void +qh__vec3_multiply(qh_vec3_t* a, double v) +{ + a->x *= v; + a->y *= v; + a->z *= v; +} + +static int +qh__vertex_equals_epsilon(qh_vertex_t* a, qh_vertex_t* b, double epsilon) +{ + return fabs(a->x - b->x) <= epsilon && + fabs(a->y - b->y) <= epsilon && + fabs(a->z - b->z) <= epsilon; +} + +static double +qh__vec3_length2(qh_vec3_t* v) +{ + return v->x * v->x + v->y * v->y + v->z * v->z; +} + +static double +qh__vec3_dot(qh_vec3_t* v1, qh_vec3_t* v2) +{ + return v1->x * v2->x + v1->y * v2->y + v1->z * v2->z; +} + +static void +qh__vec3_normalize(qh_vec3_t* v) +{ + qh__vec3_multiply(v, 1.f / sqrt(qh__vec3_length2(v))); +} + +static void +qh__find_2dps_6eps(qh_vertex_t* vertices, qh_index_t* eps, + int* ii, int* jj) +{ + int i, j; + double max = -QH_FLT_MAX; + + for (i = 0; i < 6; ++i) { + for (j = 0; j < 6; ++j) { + qh_vertex_t d; + double d2; + + if (i == j) { + continue; + } + + d = vertices[eps[i]]; + qh__vec3_sub(&d, &vertices[eps[j]]); + d2 = qh__vec3_length2(&d); + + if (d2 > max) { + *ii = i; + *jj = j; + max = d2; + } + } + } +} + +static qh_vec3_t +qh__vec3_cross(qh_vec3_t* v1, qh_vec3_t* v2) +{ + qh_vec3_t cross; + + cross.x = v1->y * v2->z - v1->z * v2->y; + cross.y = v1->z * v2->x - v1->x * v2->z; + cross.z = v1->x * v2->y - v1->y * v2->x; + + return cross; +} + +static qh_vertex_t +qh__face_centroid(qh_index_t vertices[3], qh_context_t* context) +{ + qh_vertex_t centroid; + int i; + + centroid.x = centroid.y = centroid.z = 0.0; + for (i = 0; i < 3; ++i) { + qh__vec3_add(¢roid, context->vertices + vertices[i]); + } + + qh__vec3_multiply(¢roid, 1.0 / 3.0); + + return centroid; +} + +static double +qh__dist_point_plane(qh_vertex_t* v, qh_vec3_t* normal, double sdist) +{ + return fabs(qh__vec3_dot(v, normal) - sdist); +} + +static void +qh__init_half_edge(qh_half_edge_t* half_edge) { + half_edge->adjacent_face = -1; + half_edge->he = -1; + half_edge->next_he = -1; + half_edge->opposite_he = -1; + half_edge->to_vertex = -1; + half_edge->previous_he = -1; +} + +static qh_half_edge_t* +qh__next_edge(qh_context_t* context) +{ + qh_half_edge_t* edge = context->edges + context->nedges; + qh__init_half_edge(edge); + + edge->he = context->nedges; + context->nedges++; + + QH_ASSERT(context->nedges < context->maxedges); + + return edge; +} + +static qh_face_t* +qh__next_face(qh_context_t* context) +{ + qh_face_t* face = context->faces + context->nfaces; + + face->face = context->nfaces; + face->iset.indices = NULL; + context->valid[context->nfaces] = 1; + context->nfaces++; + + QH_ASSERT(context->nfaces < context->maxfaces); + + return face; +} + +static qh_vec3_t +qh__edge_vec3(qh_half_edge_t* edge, qh_context_t* context) +{ + qh_half_edge_t prevhe = context->edges[edge->previous_he]; + qh_vec3_t v0, v1; + + v0 = context->vertices[prevhe.to_vertex]; + v1 = context->vertices[edge->to_vertex]; + + qh__vec3_sub(&v1, &v0); + qh__vec3_normalize(&v1); + + return v1; +} + +static void +qh__face_init(qh_face_t* face, qh_index_t vertices[3], + qh_context_t* context) +{ + qh_half_edge_t* e0 = qh__next_edge(context); + qh_half_edge_t* e1 = qh__next_edge(context); + qh_half_edge_t* e2 = qh__next_edge(context); + qh_vec3_t v0, v1; + qh_vertex_t centroid, normal; + + e2->to_vertex = vertices[0]; + e0->to_vertex = vertices[1]; + e1->to_vertex = vertices[2]; + + e0->next_he = e1->he; + e2->previous_he = e1->he; + face->edges[1] = e1->he; + + e1->next_he = e2->he; + e0->previous_he = e2->he; + face->edges[2] = e2->he; + v1 = qh__edge_vec3(e2, context); + + e2->next_he = e0->he; + e1->previous_he = e0->he; + face->edges[0] = e0->he; + v0 = qh__edge_vec3(e0, context); + + e2->adjacent_face = face->face; + e1->adjacent_face = face->face; + e0->adjacent_face = face->face; + + qh__vec3_multiply(&v1, -1.f); + normal = qh__vec3_cross(&v0, &v1); + + qh__vec3_normalize(&normal); + centroid = qh__face_centroid(vertices, context); + face->centroid = centroid; + face->sdist = qh__vec3_dot(&normal, ¢roid); + face->normal = normal; + face->iset.indices = QH_MALLOC(qh_index_t, QH_VERTEX_SET_SIZE); + face->iset.capacity = QH_VERTEX_SET_SIZE; + face->iset.size = 0; + face->visitededges = 0; +} + +static void +qh__tetrahedron_basis(qh_context_t* context, qh_index_t vertices[3]) +{ + qh_index_t eps[6]; + int i, j = 0, k = 0, l = 0; + double max = -QH_FLT_MAX; + + qh__find_6eps(context->vertices, context->nvertices, eps); + qh__find_2dps_6eps(context->vertices, eps, &j, &k); + + for (i = 0; i < 6; ++i) { + double d2; + + if (i == j || i == k) { + continue; + } + + d2 = qh__vertex_segment_length2(context->vertices + eps[i], + context->vertices + eps[j], + context->vertices + eps[k]); + + if (d2 > max) { + max = d2; + l = i; + } + } + + vertices[0] = eps[j]; + vertices[1] = eps[k]; + vertices[2] = eps[l]; +} + +static void +qh__push_stack(qh_index_stack_t* stack, qh_index_t index) +{ + stack->begin[stack->size] = index; + stack->size++; +} + +static qh_index_t +qh__pop_stack(qh_index_stack_t* stack) +{ + qh_index_t top = -1; + + if (stack->size > 0) { + top = stack->begin[stack->size - 1]; + stack->size--; + } + + return top; +} + +static qh_index_t +qh__furthest_point_from_plane(qh_context_t* context, + qh_index_t* indices, + int nindices, + qh_vec3_t* normal, + double sdist) +{ + int i, j = -1; + double max = -QH_FLT_MAX; + + for (i = 0; i < nindices; ++i) { + qh_index_t index = indices ? *(indices + i) : i; + double dist = qh__dist_point_plane(context->vertices + index, + normal, sdist); + + if (dist > max) { + j = i; + max = dist; + } + } + + return j; +} + +static int +qh__face_can_see_vertex(qh_face_t* face, qh_vertex_t* v) +{ + qh_vec3_t tov = *v; + + qh__vec3_sub(&tov, &face->centroid); + return qh__vec3_dot(&tov, &face->normal) > 0; +} + +static int +qh__face_can_see_vertex_epsilon(qh_context_t* context, qh_face_t* face, + qh_vertex_t* v, double epsilon) +{ + double dot; + qh_vec3_t tov = *v; + + qh__vec3_sub(&tov, &face->centroid); + dot = qh__vec3_dot(&tov, &face->normal); + + if (dot > epsilon) { + return 1; + } else { + dot = fabs(dot); + + if (dot <= epsilon && dot >= 0) { + qh_vec3_t n = face->normal; + + /* allow epsilon degeneration along the face normal */ + qh__vec3_multiply(&n, epsilon); + qh__vec3_add(v, &n); + + return 1; + } + } + + return 0; +} + +static inline void +qh__assert_half_edge(qh_half_edge_t* edge, qh_context_t* context) +{ + QH_ASSERT(edge->opposite_he != -1); + QH_ASSERT(edge->he != -1); + QH_ASSERT(edge->adjacent_face != -1); + QH_ASSERT(edge->next_he != -1); + QH_ASSERT(edge->previous_he != -1); + QH_ASSERT(edge->to_vertex != -1); + QH_ASSERT(context->edges[edge->opposite_he].to_vertex != edge->to_vertex); +} + +static inline void +qh__assert_face(qh_face_t* face, qh_context_t* context) +{ + int i; + + for (i = 0; i < 3; ++i) { + qh__assert_half_edge(context->edges + face->edges[i], context); + } + + QH_ASSERT(context->valid[face->face]); +} + +#ifdef QUICKHULL_DEBUG + +void +qh__log_face(qh_context_t* context, qh_face_t const* face) { + QH_LOG("Face %ld:\n", face->face); + for (int i = 0; i < 3; ++i) { + qh_half_edge_t edge = context->edges[face->edges[i]]; + QH_LOG("\te%d %ld\n", i, edge.he); + QH_LOG("\t\te%d.opposite_he %ld\n", i, edge.opposite_he); + QH_LOG("\t\te%d.next_he %ld\n", i, edge.next_he); + QH_LOG("\t\te%d.previous_he %ld\n", i, edge.previous_he); + QH_LOG("\t\te%d.to_vertex %ld\n", i, edge.to_vertex); + QH_LOG("\t\te%d.adjacent_face %ld\n", i, edge.adjacent_face); + } + QH_LOG("\tnormal %f %f %f\n", face->normal.x, face->normal.y, + face->normal.z); + QH_LOG("\tsdist %f\n", face->sdist); + QH_LOG("\tcentroid %f %f %f\n", face->centroid.x, face->centroid.y, + face->centroid.z); +} + +static int +qh__test_hull(qh_context_t* context, double epsilon, int testiset) +{ + unsigned int i, j, k; + + for (i = 0; i < context->nvertices; ++i) { + qh_index_t vindex = i; + char valid = 1; + + for (j = 0; j < context->nfaces; ++j) { + if (!context->valid[j]) { + continue; + } + qh_face_t* face = context->faces + j; + + qh_half_edge_t* e0 = context->edges + face->edges[0]; + qh_half_edge_t* e1 = context->edges + face->edges[1]; + qh_half_edge_t* e2 = context->edges + face->edges[2]; + + if (e0->to_vertex == vindex || + e1->to_vertex == vindex || + e2->to_vertex == vindex) { + valid = 0; + break; + } + + if (testiset) { + for (k = 0; k < face->iset.size; ++k) { + if (vindex == face->iset.indices[k]) { + valid = 0; + } + } + } + } + + if (!valid) { + continue; + } + + for (j = 0; j < context->nfaces; ++j) { + if (!context->valid[j]) { + continue; + } + qh_face_t* face = context->faces + j; + + qh_vertex_t vertex = context->vertices[vindex]; + qh__vec3_sub(&vertex, &face->centroid); + if (qh__vec3_dot(&face->normal, &vertex) > epsilon) { + #ifdef QUICKHULL_DEBUG + qh__log_face(context, face); + #endif + return 0; + } + } + } + + return 1; +} +#endif /* QUICKHULL_DEBUG */ + + +static void +#ifdef QUICKHULL_DEBUG +qh__build_hull(qh_context_t* context, double epsilon, unsigned int step, + unsigned int* failurestep) +#else +qh__build_hull(qh_context_t* context, double epsilon) +#endif +{ + qh_index_t topface = qh__pop_stack(&context->facestack); + int i, j, k; + + #ifdef QUICKHULL_DEBUG + unsigned int iteration = 0; + #endif + + while (topface != -1) { + qh_face_t* face = context->faces + topface; + qh_index_t fvi, apex; + qh_vertex_t* fv; + int reversed = 0; + + #ifdef QUICKHULL_DEBUG + if (!context->valid[topface] || face->iset.size == 0 || iteration == step) + #else + if (!context->valid[topface] || face->iset.size == 0) + #endif + { + topface = qh__pop_stack(&context->facestack); + continue; + } + + #ifdef QUICKHULL_DEBUG + if (failurestep != NULL && !qh__test_hull(context, epsilon, 1)) { + if (*failurestep == 0) { + *failurestep = iteration; + break; + } + } + + iteration++; + #endif + + fvi = qh__furthest_point_from_plane(context, face->iset.indices, + face->iset.size, &face->normal, face->sdist); + fv = context->vertices + *(face->iset.indices + fvi); + + qh__assert_face(face, context); + + /* Reset visited flag for faces */ + { + for (i = 0; i < context->nfaces; ++i) { + context->faces[i].visitededges = 0; + } + } + + /* Find horizon edge */ + { + qh_index_t tovisit = topface; + qh_face_t* facetovisit = context->faces + tovisit; + + /* Release scratch */ + context->scratch.size = 0; + + while (tovisit != -1) { + if (facetovisit->visitededges >= 3) { + context->valid[tovisit] = 0; + tovisit = qh__pop_stack(&context->scratch); + facetovisit = context->faces + tovisit; + } else { + qh_index_t edgeindex = facetovisit->edges[facetovisit->visitededges]; + qh_half_edge_t* edge; + qh_half_edge_t* oppedge; + qh_face_t* adjface; + + facetovisit->visitededges++; + + edge = context->edges + edgeindex; + oppedge = context->edges + edge->opposite_he; + adjface = context->faces + oppedge->adjacent_face; + + if (!context->valid[oppedge->adjacent_face]) { continue; } + + qh__assert_half_edge(oppedge, context); + qh__assert_half_edge(edge, context); + qh__assert_face(adjface, context); + + if (!qh__face_can_see_vertex(adjface, fv)) { + qh__push_stack(&context->horizonedges, edge->he); + } else { + context->valid[tovisit] = 0; + qh__push_stack(&context->scratch, adjface->face); + } + } + } + } + + apex = face->iset.indices[fvi]; + + /* Sort horizon edges in CCW order */ + { + qh_vertex_t triangle[3]; + int vindex = 0; + qh_vec3_t v0, v1, toapex; + qh_vertex_t n; + + for (i = 0; i < context->horizonedges.size; ++i) { + qh_index_t he0 = context->horizonedges.begin[i]; + qh_index_t he0vert = context->edges[he0].to_vertex; + qh_index_t phe0 = context->edges[he0].previous_he; + qh_index_t phe0vert = context->edges[phe0].to_vertex; + + for (j = i + 2; j < context->horizonedges.size; ++j) { + qh_index_t he1 = context->horizonedges.begin[j]; + qh_index_t he1vert = context->edges[he1].to_vertex; + qh_index_t phe1 = context->edges[he1].previous_he; + qh_index_t phe1vert = context->edges[phe1].to_vertex; + + if (phe1vert == he0vert || phe0vert == he1vert) { + QH_SWAP(qh_index_t, context->horizonedges.begin[j], + context->horizonedges.begin[i + 1]); + break; + } + } + + if (vindex < 3) { + triangle[vindex++] = + context->vertices[context->edges[he0].to_vertex]; + } + } + + if (vindex == 3) { + /* Detect first triangle face ordering */ + v0 = triangle[0]; + v1 = triangle[2]; + + qh__vec3_sub(&v0, &triangle[1]); + qh__vec3_sub(&v1, &triangle[1]); + + n = qh__vec3_cross(&v0, &v1); + + /* Get the vector to the apex */ + toapex = triangle[0]; + + qh__vec3_sub(&toapex, context->vertices + apex); + + reversed = qh__vec3_dot(&n, &toapex) < 0.f; + } + } + + /* Create new faces */ + { + qh_index_t top = qh__pop_stack(&context->horizonedges); + qh_index_t last = qh__pop_stack(&context->horizonedges); + qh_index_t first = top; + int looped = 0; + + QH_ASSERT(context->newhorizonedges.size == 0); + + /* Release scratch */ + context->scratch.size = 0; + + while (!looped) { + qh_half_edge_t* prevhe; + qh_half_edge_t* nexthe; + qh_half_edge_t* oppedge; + /* qh_vec3_t normal; */ + /* qh_vertex_t fcentroid; */ + qh_index_t verts[3]; + qh_face_t* newface; + + if (last == -1) { + looped = 1; + last = first; + } + + prevhe = context->edges + last; + nexthe = context->edges + top; + + if (reversed) { + QH_SWAP(qh_half_edge_t*, prevhe, nexthe); + } + + verts[0] = prevhe->to_vertex; + verts[1] = nexthe->to_vertex; + verts[2] = apex; + + context->valid[nexthe->adjacent_face] = 0; + + oppedge = context->edges + nexthe->opposite_he; + newface = qh__next_face(context); + + qh__face_init(newface, verts, context); + + oppedge->opposite_he = context->edges[newface->edges[0]].he; + context->edges[newface->edges[0]].opposite_he = oppedge->he; + + qh__push_stack(&context->scratch, newface->face); + qh__push_stack(&context->newhorizonedges, newface->edges[0]); + + top = last; + last = qh__pop_stack(&context->horizonedges); + } + } + + /* Attach point sets to newly created faces */ + { + for (k = 0; k < context->nfaces; ++k) { + qh_face_t* f = context->faces + k; + + if (context->valid[k] || f->iset.size == 0) { + continue; + } + + if (f->visitededges == 3) { + context->valid[k] = 0; + } + + for (i = 0; i < f->iset.size; ++i) { + qh_index_t vertex = f->iset.indices[i]; + /* qh_vertex_t* v = context->vertices + vertex; */ + qh_face_t* dface = NULL; + + for (j = 0; j < context->scratch.size; ++j) { + qh_face_t* newface = context->faces + context->scratch.begin[j]; + qh_half_edge_t* e0 = context->edges + newface->edges[0]; + qh_half_edge_t* e1 = context->edges + newface->edges[1]; + qh_half_edge_t* e2 = context->edges + newface->edges[2]; + /* qh_vertex_t cv; */ + + if (e0->to_vertex == vertex || + e1->to_vertex == vertex || + e2->to_vertex == vertex) { + continue; + } + + if (qh__face_can_see_vertex_epsilon(context, newface, + context->vertices + vertex, + epsilon)) { + dface = newface; + break; + } + } + + if (dface) { + if (dface->iset.size + 1 >= dface->iset.capacity) { + dface->iset.capacity *= 2; + dface->iset.indices = QH_REALLOC(qh_index_t, + dface->iset.indices, dface->iset.capacity); + } + + dface->iset.indices[dface->iset.size++] = vertex; + } + } + + f->iset.size = 0; + } + } + + /* Link new faces together */ + { + for (i = 0; i < context->newhorizonedges.size; ++i) { + qh_index_t phe0, nhe1; + qh_half_edge_t* he0; + qh_half_edge_t* he1; + int ii; + + if (reversed) { + ii = (i == 0) ? context->newhorizonedges.size - 1 : (i-1); + } else { + ii = (i+1) % context->newhorizonedges.size; + } + + phe0 = context->edges[context->newhorizonedges.begin[i]].previous_he; + nhe1 = context->edges[context->newhorizonedges.begin[ii]].next_he; + + he0 = context->edges + phe0; + he1 = context->edges + nhe1; + + QH_ASSERT(he1->to_vertex == apex); + QH_ASSERT(he0->opposite_he == -1); + QH_ASSERT(he1->opposite_he == -1); + + he0->opposite_he = he1->he; + he1->opposite_he = he0->he; + } + + context->newhorizonedges.size = 0; + } + + /* Push new face to stack */ + { + for (i = 0; i < context->scratch.size; ++i) { + qh_face_t* face = context->faces + context->scratch.begin[i]; + + if (face->iset.size > 0) { + qh__push_stack(&context->facestack, face->face); + } + } + + /* Release scratch */ + context->scratch.size = 0; + } + + topface = qh__pop_stack(&context->facestack); + + /* TODO: push all non-valid faces for reuse in face stack memory pool */ + } +} + +#ifdef QUICKHULL_FILES +void +qh_mesh_export(qh_mesh_t const* mesh, char const* filename) +{ + FILE* objfile = fopen(filename, "wt"); + fprintf(objfile, "o\n"); + + for (int i = 0; i < mesh->nvertices; ++i) { + qh_vertex_t v = mesh->vertices[i]; + fprintf(objfile, "v %f %f %f\n", v.x, v.y, v.z); + } + + for (int i = 0; i < mesh->nnormals; ++i) { + qh_vec3_t n = mesh->normals[i]; + fprintf(objfile, "vn %f %f %f\n", n.x, n.y, n.z); + } + + for (int i = 0, j = 0; i < mesh->nindices; i += 3, j++) { + fprintf(objfile, "f %u/%u %u/%u %u/%u\n", + mesh->indices[i+0] + 1, mesh->normalindices[j] + 1, + mesh->indices[i+1] + 1, mesh->normalindices[j] + 1, + mesh->indices[i+2] + 1, mesh->normalindices[j] + 1); + } + + fclose(objfile); +} +#endif /* QUICKHULL_FILES */ + +static qh_face_t* +qh__build_tetrahedron(qh_context_t* context, double epsilon) +{ + int i, j; + qh_index_t vertices[3]; + qh_index_t apex; + qh_face_t* faces; + qh_vertex_t normal, centroid, vapex, tcentroid; + + /* Get the initial tetrahedron basis (first face) */ + qh__tetrahedron_basis(context, &vertices[0]); + + /* Find apex from the tetrahedron basis */ + { + double sdist; + qh_vec3_t v0, v1; + + v0 = context->vertices[vertices[1]]; + v1 = context->vertices[vertices[2]]; + + qh__vec3_sub(&v0, context->vertices + vertices[0]); + qh__vec3_sub(&v1, context->vertices + vertices[0]); + + normal = qh__vec3_cross(&v0, &v1); + qh__vec3_normalize(&normal); + + centroid = qh__face_centroid(vertices, context); + sdist = qh__vec3_dot(&normal, ¢roid); + + apex = qh__furthest_point_from_plane(context, NULL, + context->nvertices, &normal, sdist); + vapex = context->vertices[apex]; + + qh__vec3_sub(&vapex, ¢roid); + + /* Whether the face is looking towards the apex */ + if (qh__vec3_dot(&vapex, &normal) > 0) { + QH_SWAP(qh_index_t, vertices[1], vertices[2]); + } + } + + faces = qh__next_face(context); + qh__face_init(&faces[0], vertices, context); + + /* Build faces from the tetrahedron basis to the apex */ + { + qh_index_t facevertices[3]; + for (i = 0; i < 3; ++i) { + qh_half_edge_t* edge = context->edges + faces[0].edges[i]; + qh_half_edge_t prevedge = context->edges[edge->previous_he]; + qh_face_t* face = faces+i+1; + qh_half_edge_t* e0; + + facevertices[0] = edge->to_vertex; + facevertices[1] = prevedge.to_vertex; + facevertices[2] = apex; + + qh__next_face(context); + qh__face_init(face, facevertices, context); + + e0 = context->edges + faces[i+1].edges[0]; + edge->opposite_he = e0->he; + e0->opposite_he = edge->he; + } + } + + /* Attach half edges to faces tied to the apex */ + { + for (i = 0; i < 3; ++i) { + qh_face_t* face; + qh_face_t* nextface; + qh_half_edge_t* e1; + qh_half_edge_t* e2; + + j = (i+2) % 3; + + face = faces+i+1; + nextface = faces+j+1; + + e1 = context->edges + face->edges[1]; + e2 = context->edges + nextface->edges[2]; + + QH_ASSERT(e1->opposite_he == -1); + QH_ASSERT(e2->opposite_he == -1); + + e1->opposite_he = e2->he; + e2->opposite_he = e1->he; + + qh__assert_half_edge(e1, context); + qh__assert_half_edge(e2, context); + } + } + + /* Create initial point set; every point is */ + /* attached to the first face it can see */ + { + for (i = 0; i < context->nvertices; ++i) { + /* qh_vertex_t* v; */ + qh_face_t* dface = NULL; + + if (vertices[0] == i || vertices[1] == i || vertices[2] == i) { + continue; + } + + for (j = 0; j < 4; ++j) { + if (qh__face_can_see_vertex_epsilon(context, context->faces + j, + context->vertices + i, epsilon)) { + dface = context->faces + j; + break; + } + } + + if (dface) { + int valid = 1; + int j; + + for (j = 0; j < 3; ++j) { + qh_half_edge_t* e = context->edges + dface->edges[j]; + if (i == e->to_vertex) { + valid = 0; + break; + } + } + + if (!valid) { continue; } + + if (dface->iset.size + 1 >= dface->iset.capacity) { + dface->iset.capacity *= 2; + dface->iset.indices = QH_REALLOC(qh_index_t, + dface->iset.indices, dface->iset.capacity); + } + + dface->iset.indices[dface->iset.size++] = i; + } + } + } + + /* Add initial tetrahedron faces to the face stack */ + tcentroid.x = tcentroid.y = tcentroid.z = 0.0; + for (i = 0; i < 4; ++i) { + context->valid[i] = 1; + qh__assert_face(context->faces + i, context); + qh__push_stack(&context->facestack, i); + qh__vec3_add(&tcentroid, &context->faces[i].centroid); + } + + /* Assign the tetrahedron centroid */ + qh__vec3_multiply(&tcentroid, 0.25); + context->centroid = tcentroid; + + QH_ASSERT(context->nedges == context->nfaces * 3); + QH_ASSERT(context->nfaces == 4); + QH_ASSERT(context->facestack.size == 4); + + return faces; +} + +static void +qh__remove_vertex_duplicates(qh_context_t* context, double epsilon) +{ + int i, j, k; + for (i = 0; i < context->nvertices; ++i) { + qh_vertex_t* v = context->vertices + i; + if (v->x == 0) v->x = 0; + if (v->y == 0) v->y = 0; + if (v->z == 0) v->z = 0; + for (j = i + 1; j < context->nvertices; ++j) { + if (qh__vertex_equals_epsilon(context->vertices + i, + context->vertices + j, epsilon)) + { + for (k = j; k < context->nvertices - 1; ++k) { + context->vertices[k] = context->vertices[k+1]; + } + context->nvertices--; + } + } + } +} + +static void +qh__init_context(qh_context_t* context, qh_vertex_t const* vertices, + unsigned int nvertices) +{ + /* TODO: */ + /* size_t nedges = 3 * nvertices - 6; */ + /* size_t nfaces = 2 * nvertices - 4; */ + unsigned int nfaces = nvertices * (nvertices - 1); + unsigned int nedges = nfaces * 3; + + context->edges = QH_MALLOC(qh_half_edge_t, nedges); + context->faces = QH_MALLOC(qh_face_t, nfaces); + context->facestack.begin = QH_MALLOC(qh_index_t, nfaces); + context->scratch.begin = QH_MALLOC(qh_index_t, nfaces); + context->horizonedges.begin = QH_MALLOC(qh_index_t, nedges); + context->newhorizonedges.begin = QH_MALLOC(qh_index_t, nedges); + context->valid = QH_MALLOC(char, nfaces); + + context->vertices = QH_MALLOC(qh_vertex_t, nvertices); + memcpy(context->vertices, vertices, sizeof(qh_vertex_t) * nvertices); + + context->nvertices = nvertices; + context->nedges = 0; + context->nfaces = 0; + context->facestack.size = 0; + context->scratch.size = 0; + context->horizonedges.size = 0; + context->newhorizonedges.size = 0; + + #ifdef QUICKHULL_DEBUG + context->maxfaces = nfaces; + context->maxedges = nedges; + #endif +} + +static void +qh__free_context(qh_context_t* context) +{ + int i; + + for (i = 0; i < context->nfaces; ++i) { + QH_FREE(context->faces[i].iset.indices); + context->faces[i].iset.size = 0; + } + + context->nvertices = 0; + context->nfaces = 0; + + QH_FREE(context->edges); + + QH_FREE(context->faces); + QH_FREE(context->facestack.begin); + QH_FREE(context->scratch.begin); + QH_FREE(context->horizonedges.begin); + QH_FREE(context->newhorizonedges.begin); + QH_FREE(context->vertices); + QH_FREE(context->valid); +} + +void +qh_free_mesh(qh_mesh_t mesh) +{ + QH_FREE(mesh.vertices); + QH_FREE(mesh.indices); + QH_FREE(mesh.normalindices); + QH_FREE(mesh.normals); +} + +static double +qh__compute_epsilon(qh_vertex_t const* vertices, unsigned int nvertices) +{ + double epsilon; + unsigned int i; + + double maxxi = -QH_FLT_MAX; + double maxyi = -QH_FLT_MAX; + + for (i = 0; i < nvertices; ++i) { + double fxi = fabs(vertices[i].x); + double fyi = fabs(vertices[i].y); + + if (fxi > maxxi) { + maxxi = fxi; + } + if (fyi > maxyi) { + maxyi = fyi; + } + } + + epsilon = 2 * (maxxi + maxyi) * QH_FLT_EPS; + + return epsilon; +} + +qh_mesh_t +qh_quickhull3d(qh_vertex_t const* vertices, unsigned int nvertices) +{ + qh_mesh_t m; + qh_context_t context; + /* unsigned int* indices; */ + unsigned int nfaces = 0, i, index, nindices; + double epsilon; + + epsilon = qh__compute_epsilon(vertices, nvertices); + + qh__init_context(&context, vertices, nvertices); + + qh__remove_vertex_duplicates(&context, epsilon); + + /* Build the initial tetrahedron */ + qh__build_tetrahedron(&context, epsilon); + + /* Build the convex hull */ + #ifdef QUICKHULL_DEBUG + qh__build_hull(&context, epsilon, -1, NULL); + #else + qh__build_hull(&context, epsilon); + #endif + + /* QH_ASSERT(qh__test_hull(&context, epsilon)); */ + + for (i = 0; i < context.nfaces; ++i) { + if (context.valid[i]) { nfaces++; } + } + + nindices = nfaces * 3; + + m.normals = QH_MALLOC(qh_vertex_t, nfaces); + m.normalindices = QH_MALLOC(unsigned int, nfaces); + m.vertices = QH_MALLOC(qh_vertex_t, nindices); + m.indices = QH_MALLOC(unsigned int, nindices); + m.nindices = nindices; + m.nnormals = nfaces; + m.nvertices = 0; + + { + index = 0; + for (i = 0; i < context.nfaces; ++i) { + if (!context.valid[i]) { continue; } + m.normals[index] = context.faces[i].normal; + index++; + } + + index = 0; + for (i = 0; i < context.nfaces; ++i) { + if (!context.valid[i]) { continue; } + m.normalindices[index] = index; + index++; + } + + index = 0; + for (i = 0; i < context.nfaces; ++i) { + if (!context.valid[i]) { continue; } + m.indices[index+0] = index+0; + m.indices[index+1] = index+1; + m.indices[index+2] = index+2; + index += 3; + } + + for (i = 0; i < context.nfaces; ++i) { + if (!context.valid[i]) { continue; } + qh_half_edge_t e0 = context.edges[context.faces[i].edges[0]]; + qh_half_edge_t e1 = context.edges[context.faces[i].edges[1]]; + qh_half_edge_t e2 = context.edges[context.faces[i].edges[2]]; + + m.vertices[m.nvertices++] = context.vertices[e0.to_vertex]; + m.vertices[m.nvertices++] = context.vertices[e1.to_vertex]; + m.vertices[m.nvertices++] = context.vertices[e2.to_vertex]; + } + } + + qh__free_context(&context); + + return m; +}