path: root/hacks/glx/quickhull.c

/* 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 */

extern const char *progname;

#include "quickhull.h"

#include "screenhackI.h" /* for jwxyz_abort */

#include <math.h>   /* sqrt & fabs */
#include <stdio.h>  /* FILE */
#include <string.h> /* memcpy */

/* Quickhull helpers, define your own if needed */
#ifndef QUICKHULL_HELPERS
#include <stdlib.h> /* 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(&centroid, context->vertices + vertices[i]);
  }

  qh__vec3_multiply(&centroid, 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, &centroid);
  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, &centroid);

    apex = qh__furthest_point_from_plane(context, NULL,
      context->nvertices, &normal, sdist);
    vapex = context->vertices[apex];

    qh__vec3_sub(&vapex, &centroid);

    /* 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);

  if (!(context->edges &&
        context->faces &&
        context->facestack.begin &&
        context->scratch.begin &&
        context->horizonedges.begin &&
        context->newhorizonedges.begin &&
        context->valid)) {
# ifdef HAVE_JWXYZ
    jwxyz_abort ("%s: out of memory", progname);
# else
    fprintf (stderr, "%s: out of memory\n", progname);
    exit (1);
# endif
  }


  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;
}