git.hungrycats.org Git - xscreensaver/blob - hacks/glx/quickhull.c

   1 /* quickhull, Copyright (c) 2016 Karim Naaji, karim.naaji@gmail.com
   2    https://github.com/karimnaaji/3d-quickhull
   3
   4  LICENCE:
   5   The MIT License (MIT)
   6
   7   Copyright (c) 2016 Karim Naaji, karim.naaji@gmail.com
   8
   9   Permission is hereby granted, free of charge, to any person obtaining a copy
  10   of this software and associated documentation files (the "Software"), to deal
  11   in the Software without restriction, including without limitation the rights
  12   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  13   copies of the Software, and to permit persons to whom the Software is
  14   furnished to do so, subject to the following conditions:
  15
  16   The above copyright notice and this permission notice shall be included in
  17   all copies or substantial portions of the Software.
  18
  19   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  20   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  21   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  22   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  23   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  24   OUT OF OR IN CONNECTION WITH THE
  25
  26  REFERENCES:
  27   [1] http://box2d.org/files/GDC2014/DirkGregorius_ImplementingQuickHull.pdf
  28   [2] http://www.cs.smith.edu/~orourke/books/compgeom.html
  29   [3] http://www.flipcode.com/archives/The_Half-Edge_Data_Structure.shtml
  30   [4] http://doc.cgal.org/latest/HalfedgeDS/index.html
  31   [5] http://thomasdiewald.com/blog/?p=1888
  32   [6] https://fgiesen.wordpress.com/2012/02/21/half-edge-based-mesh-representations-theory/
  33
  34  HOWTO:
  35   #define QUICKHULL_DEBUG // Only if assertions need to be checked
  36   #include "quickhull.h"
  37
  38  HISTORY:
  39   - 25-Feb-2018: jwz: adapted for xscreensaver
  40   - 1.0.1 (2016-11-01): Various improvements over epsilon issues and
  41             degenerate faces
  42             Debug functionalities to test final results dynamically
  43             API to export hull meshes in OBJ files
  44   - 1.0   (2016-09-10): Initial
  45
  46  TODO:
  47   - use float* from public interface
  48   - reduce memory usage
  49 */
  50
  51 #ifdef HAVE_CONFIG_H
  52 # include "config.h"
  53 #endif /* HAVE_CONFIG_H */
  54
  55 extern const char *progname;
  56
  57 #include "quickhull.h"
  58
  59 #include "screenhackI.h" /* for jwxyz_abort */
  60
  61 #include <math.h>   /* sqrt & fabs */
  62 #include <stdio.h>  /* FILE */
  63 #include <string.h> /* memcpy */
  64
  65 /* Quickhull helpers, define your own if needed */
  66 #ifndef QUICKHULL_HELPERS
  67 #include <stdlib.h> /* malloc, free, realloc */
  68 #define QUICKHULL_HELPERS 1
  69 #define QH_MALLOC(T, N) ((T*) malloc(N * sizeof(T)))
  70 #define QH_REALLOC(T, P, N) ((T*)realloc(P, sizeof(T) * N))
  71 #define QH_FREE(T) free(T)
  72 #define QH_SWAP(T, A, B) { T tmp = B; B = A; A = tmp; }
  73 #ifdef QUICKHULL_DEBUG
  74 #define QH_ASSERT(STMT) if (!(STMT)) { *(int *)0 = 0; }
  75 #define QH_LOG(FMT, ...) printf(FMT, ## __VA_ARGS__)
  76 #else
  77 #define QH_ASSERT(STMT)
  78 #define QH_LOG(FMT, ...)
  79 #endif /* QUICKHULL_DEBUG */
  80 #endif /* QUICKHULL_HELPERS */
  81
  82 #ifndef QH_FLT_MAX
  83 #define QH_FLT_MAX 1e+37F
  84 #endif
  85
  86 #ifndef QH_FLT_EPS
  87 #define QH_FLT_EPS 1E-5F
  88 #endif
  89
  90 #ifndef QH_VERTEX_SET_SIZE
  91 #define QH_VERTEX_SET_SIZE 128
  92 #endif
  93
  94 typedef long qh_index_t;
  95
  96 typedef struct qh_half_edge {
  97   qh_index_t opposite_he;   /* index of the opposite half edge */
  98   qh_index_t next_he;     /* index of the next half edge */
  99   qh_index_t previous_he;   /* index of the previous half edge */
 100   qh_index_t he;        /* index of the current half edge */
 101   qh_index_t to_vertex;     /* index of the next vertex */
 102   qh_index_t adjacent_face;   /* index of the ajacent face */
 103 } qh_half_edge_t;
 104
 105 typedef struct qh_index_set {
 106   qh_index_t* indices;
 107   unsigned int size;
 108   unsigned int capacity;
 109 } qh_index_set_t;
 110
 111 typedef struct qh_face {
 112   qh_index_set_t iset;
 113   qh_vec3_t normal;
 114   qh_vertex_t centroid;
 115   qh_index_t edges[3];
 116   qh_index_t face;
 117   double sdist;
 118   int visitededges;
 119 } qh_face_t;
 120
 121 typedef struct qh_index_stack {
 122   qh_index_t* begin;
 123   unsigned int size;
 124 } qh_index_stack_t;
 125
 126 typedef struct qh_context {
 127   qh_face_t* faces;
 128   qh_half_edge_t* edges;
 129   qh_vertex_t* vertices;
 130   qh_vertex_t centroid;
 131   qh_index_stack_t facestack;
 132   qh_index_stack_t scratch;
 133   qh_index_stack_t horizonedges;
 134   qh_index_stack_t newhorizonedges;
 135   char* valid;
 136   unsigned int nedges;
 137   unsigned int nvertices;
 138   unsigned int nfaces;
 139
 140   #ifdef QUICKHULL_DEBUG
 141   unsigned int maxfaces;
 142   unsigned int maxedges;
 143   #endif
 144 } qh_context_t;
 145
 146 static void
 147 qh__find_6eps(qh_vertex_t* vertices, unsigned int nvertices, qh_index_t* eps)
 148 {
 149   qh_vertex_t* ptr = vertices;
 150
 151   double minxy = +QH_FLT_MAX;
 152   double minxz = +QH_FLT_MAX;
 153   double minyz = +QH_FLT_MAX;
 154
 155   double maxxy = -QH_FLT_MAX;
 156   double maxxz = -QH_FLT_MAX;
 157   double maxyz = -QH_FLT_MAX;
 158
 159   int i = 0;
 160   for (i = 0; i < 6; ++i) {
 161     eps[i] = 0;
 162   }
 163
 164   for (i = 0; i < nvertices; ++i) {
 165     if (ptr->z < minxy) {
 166       eps[0] = i;
 167       minxy = ptr->z;
 168     }
 169     if (ptr->y < minxz) {
 170       eps[1] = i;
 171       minxz = ptr->y;
 172     }
 173     if (ptr->x < minyz) {
 174       eps[2] = i;
 175       minyz = ptr->x;
 176     }
 177     if (ptr->z > maxxy) {
 178       eps[3] = i;
 179       maxxy = ptr->z;
 180     }
 181     if (ptr->y > maxxz) {
 182       eps[4] = i;
 183       maxxz = ptr->y;
 184     }
 185     if (ptr->x > maxyz) {
 186       eps[5] = i;
 187       maxyz = ptr->x;
 188     }
 189     ptr++;
 190   }
 191 }
 192
 193 static double
 194 qh__vertex_segment_length2(qh_vertex_t* p, qh_vertex_t* a, qh_vertex_t* b)
 195 {
 196   double dx = b->x - a->x;
 197   double dy = b->y - a->y;
 198   double dz = b->z - a->z;
 199
 200   double d = dx * dx + dy * dy + dz * dz;
 201
 202   double x = a->x;
 203   double y = a->y;
 204   double z = a->z;
 205
 206   if (d != 0) {
 207     double t = ((p->x - a->x) * dx +
 208       (p->y - a->y) * dy +
 209       (p->z - a->z) * dz) / d;
 210
 211     if (t > 1) {
 212       x = b->x;
 213       y = b->y;
 214       z = b->z;
 215     } else if (t > 0) {
 216       x += dx * t;
 217       y += dy * t;
 218       z += dz * t;
 219     }
 220   }
 221
 222   dx = p->x - x;
 223   dy = p->y - y;
 224   dz = p->z - z;
 225
 226   return dx * dx + dy * dy + dz * dz;
 227 }
 228
 229 static void
 230 qh__vec3_sub(qh_vec3_t* a, qh_vec3_t* b)
 231 {
 232   a->x -= b->x;
 233   a->y -= b->y;
 234   a->z -= b->z;
 235 }
 236
 237 static void
 238 qh__vec3_add(qh_vec3_t* a, qh_vec3_t* b)
 239 {
 240   a->x += b->x;
 241   a->y += b->y;
 242   a->z += b->z;
 243 }
 244
 245 static void
 246 qh__vec3_multiply(qh_vec3_t* a, double v)
 247 {
 248   a->x *= v;
 249   a->y *= v;
 250   a->z *= v;
 251 }
 252
 253 static int
 254 qh__vertex_equals_epsilon(qh_vertex_t* a, qh_vertex_t* b, double epsilon)
 255 {
 256   return fabs(a->x - b->x) <= epsilon &&
 257        fabs(a->y - b->y) <= epsilon &&
 258        fabs(a->z - b->z) <= epsilon;
 259 }
 260
 261 static double
 262 qh__vec3_length2(qh_vec3_t* v)
 263 {
 264   return v->x * v->x + v->y * v->y + v->z * v->z;
 265 }
 266
 267 static double
 268 qh__vec3_dot(qh_vec3_t* v1, qh_vec3_t* v2)
 269 {
 270   return v1->x * v2->x + v1->y * v2->y + v1->z * v2->z;
 271 }
 272
 273 static void
 274 qh__vec3_normalize(qh_vec3_t* v)
 275 {
 276   qh__vec3_multiply(v, 1.f / sqrt(qh__vec3_length2(v)));
 277 }
 278
 279 static void
 280 qh__find_2dps_6eps(qh_vertex_t* vertices, qh_index_t* eps,
 281                         int* ii, int* jj)
 282 {
 283   int i, j;
 284   double max = -QH_FLT_MAX;
 285
 286   for (i = 0; i < 6; ++i) {
 287     for (j = 0; j < 6; ++j) {
 288       qh_vertex_t d;
 289       double d2;
 290
 291       if (i == j) {
 292         continue;
 293       }
 294
 295       d = vertices[eps[i]];
 296       qh__vec3_sub(&d, &vertices[eps[j]]);
 297       d2 = qh__vec3_length2(&d);
 298
 299       if (d2 > max) {
 300         *ii = i;
 301         *jj = j;
 302         max = d2;
 303       }
 304     }
 305   }
 306 }
 307
 308 static qh_vec3_t
 309 qh__vec3_cross(qh_vec3_t* v1, qh_vec3_t* v2)
 310 {
 311   qh_vec3_t cross;
 312
 313   cross.x = v1->y * v2->z - v1->z * v2->y;
 314   cross.y = v1->z * v2->x - v1->x * v2->z;
 315   cross.z = v1->x * v2->y - v1->y * v2->x;
 316
 317   return cross;
 318 }
 319
 320 static qh_vertex_t
 321 qh__face_centroid(qh_index_t vertices[3], qh_context_t* context)
 322 {
 323   qh_vertex_t centroid;
 324   int i;
 325
 326   centroid.x = centroid.y = centroid.z = 0.0;
 327   for (i = 0; i < 3; ++i) {
 328     qh__vec3_add(&centroid, context->vertices + vertices[i]);
 329   }
 330
 331   qh__vec3_multiply(&centroid, 1.0 / 3.0);
 332
 333   return centroid;
 334 }
 335
 336 static double
 337 qh__dist_point_plane(qh_vertex_t* v, qh_vec3_t* normal, double sdist)
 338 {
 339   return fabs(qh__vec3_dot(v, normal) - sdist);
 340 }
 341
 342 static void
 343 qh__init_half_edge(qh_half_edge_t* half_edge) {
 344   half_edge->adjacent_face = -1;
 345   half_edge->he = -1;
 346   half_edge->next_he = -1;
 347   half_edge->opposite_he = -1;
 348   half_edge->to_vertex = -1;
 349   half_edge->previous_he = -1;
 350 }
 351
 352 static qh_half_edge_t*
 353 qh__next_edge(qh_context_t* context)
 354 {
 355   qh_half_edge_t* edge = context->edges + context->nedges;
 356   qh__init_half_edge(edge);
 357
 358   edge->he = context->nedges;
 359   context->nedges++;
 360
 361   QH_ASSERT(context->nedges < context->maxedges);
 362
 363   return edge;
 364 }
 365
 366 static qh_face_t*
 367 qh__next_face(qh_context_t* context)
 368 {
 369   qh_face_t* face = context->faces + context->nfaces;
 370
 371   face->face = context->nfaces;
 372   face->iset.indices = NULL;
 373   context->valid[context->nfaces] = 1;
 374   context->nfaces++;
 375
 376   QH_ASSERT(context->nfaces < context->maxfaces);
 377
 378   return face;
 379 }
 380
 381 static qh_vec3_t
 382 qh__edge_vec3(qh_half_edge_t* edge, qh_context_t* context)
 383 {
 384   qh_half_edge_t prevhe = context->edges[edge->previous_he];
 385   qh_vec3_t v0, v1;
 386
 387   v0 = context->vertices[prevhe.to_vertex];
 388   v1 = context->vertices[edge->to_vertex];
 389
 390   qh__vec3_sub(&v1, &v0);
 391   qh__vec3_normalize(&v1);
 392
 393   return v1;
 394 }
 395
 396 static void
 397 qh__face_init(qh_face_t* face, qh_index_t vertices[3],
 398                    qh_context_t* context)
 399 {
 400   qh_half_edge_t* e0 = qh__next_edge(context);
 401   qh_half_edge_t* e1 = qh__next_edge(context);
 402   qh_half_edge_t* e2 = qh__next_edge(context);
 403   qh_vec3_t v0, v1;
 404   qh_vertex_t centroid, normal;
 405
 406   e2->to_vertex = vertices[0];
 407   e0->to_vertex = vertices[1];
 408   e1->to_vertex = vertices[2];
 409
 410   e0->next_he = e1->he;
 411   e2->previous_he = e1->he;
 412   face->edges[1] = e1->he;
 413
 414   e1->next_he = e2->he;
 415   e0->previous_he = e2->he;
 416   face->edges[2] = e2->he;
 417   v1 = qh__edge_vec3(e2, context);
 418
 419   e2->next_he = e0->he;
 420   e1->previous_he = e0->he;
 421   face->edges[0] = e0->he;
 422   v0 = qh__edge_vec3(e0, context);
 423
 424   e2->adjacent_face = face->face;
 425   e1->adjacent_face = face->face;
 426   e0->adjacent_face = face->face;
 427
 428   qh__vec3_multiply(&v1, -1.f);
 429   normal = qh__vec3_cross(&v0, &v1);
 430
 431   qh__vec3_normalize(&normal);
 432   centroid = qh__face_centroid(vertices, context);
 433   face->centroid = centroid;
 434   face->sdist = qh__vec3_dot(&normal, &centroid);
 435   face->normal = normal;
 436   face->iset.indices = QH_MALLOC(qh_index_t, QH_VERTEX_SET_SIZE);
 437   face->iset.capacity = QH_VERTEX_SET_SIZE;
 438   face->iset.size = 0;
 439   face->visitededges = 0;
 440 }
 441
 442 static void
 443 qh__tetrahedron_basis(qh_context_t* context, qh_index_t vertices[3])
 444 {
 445   qh_index_t eps[6];
 446   int i, j = 0, k = 0, l = 0;
 447   double max = -QH_FLT_MAX;
 448
 449   qh__find_6eps(context->vertices, context->nvertices, eps);
 450   qh__find_2dps_6eps(context->vertices, eps, &j, &k);
 451
 452   for (i = 0; i < 6; ++i) {
 453     double d2;
 454
 455     if (i == j || i == k) {
 456       continue;
 457     }
 458
 459     d2 = qh__vertex_segment_length2(context->vertices + eps[i],
 460       context->vertices + eps[j],
 461       context->vertices + eps[k]);
 462
 463     if (d2 > max) {
 464       max = d2;
 465       l = i;
 466     }
 467   }
 468
 469   vertices[0] = eps[j];
 470   vertices[1] = eps[k];
 471   vertices[2] = eps[l];
 472 }
 473
 474 static void
 475 qh__push_stack(qh_index_stack_t* stack, qh_index_t index)
 476 {
 477   stack->begin[stack->size] = index;
 478   stack->size++;
 479 }
 480
 481 static qh_index_t
 482 qh__pop_stack(qh_index_stack_t* stack)
 483 {
 484   qh_index_t top = -1;
 485
 486   if (stack->size > 0) {
 487     top = stack->begin[stack->size - 1];
 488     stack->size--;
 489   }
 490
 491   return top;
 492 }
 493
 494 static qh_index_t
 495 qh__furthest_point_from_plane(qh_context_t* context,
 496                               qh_index_t* indices,
 497                               int nindices,
 498                               qh_vec3_t* normal,
 499                               double sdist)
 500 {
 501   int i, j = -1;
 502   double max = -QH_FLT_MAX;
 503
 504   for (i = 0; i < nindices; ++i) {
 505     qh_index_t index = indices ? *(indices + i) : i;
 506     double dist = qh__dist_point_plane(context->vertices + index,
 507                                       normal, sdist);
 508
 509     if (dist > max) {
 510       j = i;
 511       max = dist;
 512     }
 513   }
 514
 515   return j;
 516 }
 517
 518 static int
 519 qh__face_can_see_vertex(qh_face_t* face, qh_vertex_t* v)
 520 {
 521   qh_vec3_t tov = *v;
 522
 523   qh__vec3_sub(&tov, &face->centroid);
 524   return qh__vec3_dot(&tov, &face->normal) > 0;
 525 }
 526
 527 static int
 528 qh__face_can_see_vertex_epsilon(qh_context_t* context, qh_face_t* face,
 529                                 qh_vertex_t* v, double epsilon)
 530 {
 531   double dot;
 532   qh_vec3_t tov = *v;
 533
 534   qh__vec3_sub(&tov, &face->centroid);
 535   dot = qh__vec3_dot(&tov, &face->normal);
 536
 537   if (dot > epsilon) {
 538     return 1;
 539   } else {
 540     dot = fabs(dot);
 541
 542     if (dot <= epsilon && dot >= 0) {
 543       qh_vec3_t n = face->normal;
 544
 545       /* allow epsilon degeneration along the face normal */
 546       qh__vec3_multiply(&n, epsilon);
 547       qh__vec3_add(v, &n);
 548
 549       return 1;
 550     }
 551   }
 552
 553   return 0;
 554 }
 555
 556 static inline void
 557 qh__assert_half_edge(qh_half_edge_t* edge, qh_context_t* context)
 558 {
 559   QH_ASSERT(edge->opposite_he != -1);
 560   QH_ASSERT(edge->he != -1);
 561   QH_ASSERT(edge->adjacent_face != -1);
 562   QH_ASSERT(edge->next_he != -1);
 563   QH_ASSERT(edge->previous_he != -1);
 564   QH_ASSERT(edge->to_vertex != -1);
 565   QH_ASSERT(context->edges[edge->opposite_he].to_vertex != edge->to_vertex);
 566 }
 567
 568 static inline void
 569 qh__assert_face(qh_face_t* face, qh_context_t* context)
 570 {
 571   int i;
 572
 573   for (i = 0; i < 3; ++i) {
 574     qh__assert_half_edge(context->edges + face->edges[i], context);
 575   }
 576
 577   QH_ASSERT(context->valid[face->face]);
 578 }
 579
 580 #ifdef QUICKHULL_DEBUG
 581
 582 void
 583 qh__log_face(qh_context_t* context, qh_face_t const* face) {
 584   QH_LOG("Face %ld:\n", face->face);
 585   for (int i = 0; i < 3; ++i) {
 586     qh_half_edge_t edge = context->edges[face->edges[i]];
 587     QH_LOG("\te%d %ld\n", i, edge.he);
 588     QH_LOG("\t\te%d.opposite_he %ld\n", i, edge.opposite_he);
 589     QH_LOG("\t\te%d.next_he %ld\n", i, edge.next_he);
 590     QH_LOG("\t\te%d.previous_he %ld\n", i, edge.previous_he);
 591     QH_LOG("\t\te%d.to_vertex %ld\n", i, edge.to_vertex);
 592     QH_LOG("\t\te%d.adjacent_face %ld\n", i, edge.adjacent_face);
 593   }
 594   QH_LOG("\tnormal %f %f %f\n", face->normal.x, face->normal.y,
 595          face->normal.z);
 596   QH_LOG("\tsdist %f\n", face->sdist);
 597   QH_LOG("\tcentroid %f %f %f\n", face->centroid.x, face->centroid.y,
 598          face->centroid.z);
 599 }
 600
 601 static int
 602 qh__test_hull(qh_context_t* context, double epsilon, int testiset)
 603 {
 604   unsigned int i, j, k;
 605
 606   for (i = 0; i < context->nvertices; ++i) {
 607     qh_index_t vindex = i;
 608     char valid = 1;
 609
 610     for (j = 0; j < context->nfaces; ++j) {
 611       if (!context->valid[j]) {
 612         continue;
 613       }
 614       qh_face_t* face = context->faces + j;
 615
 616       qh_half_edge_t* e0 = context->edges + face->edges[0];
 617       qh_half_edge_t* e1 = context->edges + face->edges[1];
 618       qh_half_edge_t* e2 = context->edges + face->edges[2];
 619
 620       if (e0->to_vertex == vindex ||
 621         e1->to_vertex == vindex ||
 622         e2->to_vertex == vindex) {
 623         valid = 0;
 624         break;
 625       }
 626
 627       if (testiset) {
 628         for (k = 0; k < face->iset.size; ++k) {
 629           if (vindex == face->iset.indices[k]) {
 630             valid = 0;
 631           }
 632         }
 633       }
 634     }
 635
 636     if (!valid) {
 637       continue;
 638     }
 639
 640     for (j = 0; j < context->nfaces; ++j) {
 641       if (!context->valid[j]) {
 642         continue;
 643       }
 644       qh_face_t* face = context->faces + j;
 645
 646       qh_vertex_t vertex = context->vertices[vindex];
 647       qh__vec3_sub(&vertex, &face->centroid);
 648       if (qh__vec3_dot(&face->normal, &vertex) > epsilon) {
 649         #ifdef QUICKHULL_DEBUG
 650         qh__log_face(context, face);
 651         #endif
 652         return 0;
 653       }
 654     }
 655   }
 656
 657   return 1;
 658 }
 659 #endif /* QUICKHULL_DEBUG */
 660
 661
 662 static void
 663 #ifdef QUICKHULL_DEBUG
 664 qh__build_hull(qh_context_t* context, double epsilon, unsigned int step,
 665                unsigned int* failurestep)
 666 #else
 667 qh__build_hull(qh_context_t* context, double epsilon)
 668 #endif
 669 {
 670   qh_index_t topface = qh__pop_stack(&context->facestack);
 671   int i, j, k;
 672
 673   #ifdef QUICKHULL_DEBUG
 674   unsigned int iteration = 0;
 675   #endif
 676
 677   while (topface != -1) {
 678     qh_face_t* face = context->faces + topface;
 679     qh_index_t fvi, apex;
 680     qh_vertex_t* fv;
 681     int reversed = 0;
 682
 683     #ifdef QUICKHULL_DEBUG
 684     if (!context->valid[topface] || face->iset.size == 0 || iteration == step)
 685     #else
 686     if (!context->valid[topface] || face->iset.size == 0)
 687     #endif
 688     {
 689       topface = qh__pop_stack(&context->facestack);
 690       continue;
 691     }
 692
 693     #ifdef QUICKHULL_DEBUG
 694     if (failurestep != NULL && !qh__test_hull(context, epsilon, 1)) {
 695       if (*failurestep == 0) {
 696         *failurestep = iteration;
 697         break;
 698       }
 699     }
 700
 701     iteration++;
 702     #endif
 703
 704     fvi = qh__furthest_point_from_plane(context, face->iset.indices,
 705       face->iset.size, &face->normal, face->sdist);
 706     fv = context->vertices + *(face->iset.indices + fvi);
 707
 708     qh__assert_face(face, context);
 709
 710     /* Reset visited flag for faces */
 711     {
 712       for (i = 0; i < context->nfaces; ++i) {
 713         context->faces[i].visitededges = 0;
 714       }
 715     }
 716
 717     /* Find horizon edge */
 718     {
 719       qh_index_t tovisit = topface;
 720       qh_face_t* facetovisit = context->faces + tovisit;
 721
 722       /* Release scratch */
 723       context->scratch.size = 0;
 724
 725       while (tovisit != -1) {
 726         if (facetovisit->visitededges >= 3) {
 727           context->valid[tovisit] = 0;
 728           tovisit = qh__pop_stack(&context->scratch);
 729           facetovisit = context->faces + tovisit;
 730         } else {
 731           qh_index_t edgeindex = facetovisit->edges[facetovisit->visitededges];
 732           qh_half_edge_t* edge;
 733           qh_half_edge_t* oppedge;
 734           qh_face_t* adjface;
 735
 736           facetovisit->visitededges++;
 737
 738           edge = context->edges + edgeindex;
 739           oppedge = context->edges + edge->opposite_he;
 740           adjface = context->faces + oppedge->adjacent_face;
 741
 742           if (!context->valid[oppedge->adjacent_face]) { continue; }
 743
 744           qh__assert_half_edge(oppedge, context);
 745           qh__assert_half_edge(edge, context);
 746           qh__assert_face(adjface, context);
 747
 748           if (!qh__face_can_see_vertex(adjface, fv)) {
 749             qh__push_stack(&context->horizonedges, edge->he);
 750           } else {
 751             context->valid[tovisit] = 0;
 752             qh__push_stack(&context->scratch, adjface->face);
 753           }
 754         }
 755       }
 756     }
 757
 758     apex = face->iset.indices[fvi];
 759
 760     /* Sort horizon edges in CCW order */
 761     {
 762       qh_vertex_t triangle[3];
 763       int vindex = 0;
 764       qh_vec3_t v0, v1, toapex;
 765       qh_vertex_t n;
 766
 767       for (i = 0; i < context->horizonedges.size; ++i) {
 768         qh_index_t he0 = context->horizonedges.begin[i];
 769         qh_index_t he0vert = context->edges[he0].to_vertex;
 770         qh_index_t phe0 = context->edges[he0].previous_he;
 771         qh_index_t phe0vert = context->edges[phe0].to_vertex;
 772
 773         for (j = i + 2; j < context->horizonedges.size; ++j) {
 774           qh_index_t he1 = context->horizonedges.begin[j];
 775           qh_index_t he1vert = context->edges[he1].to_vertex;
 776           qh_index_t phe1 = context->edges[he1].previous_he;
 777           qh_index_t phe1vert = context->edges[phe1].to_vertex;
 778
 779           if (phe1vert == he0vert || phe0vert == he1vert) {
 780             QH_SWAP(qh_index_t, context->horizonedges.begin[j],
 781                 context->horizonedges.begin[i + 1]);
 782             break;
 783           }
 784         }
 785
 786         if (vindex < 3) {
 787           triangle[vindex++] =
 788             context->vertices[context->edges[he0].to_vertex];
 789         }
 790       }
 791
 792       if (vindex == 3) {
 793         /* Detect first triangle face ordering */
 794         v0 = triangle[0];
 795         v1 = triangle[2];
 796
 797         qh__vec3_sub(&v0, &triangle[1]);
 798         qh__vec3_sub(&v1, &triangle[1]);
 799
 800         n = qh__vec3_cross(&v0, &v1);
 801
 802         /* Get the vector to the apex */
 803         toapex = triangle[0];
 804
 805         qh__vec3_sub(&toapex, context->vertices + apex);
 806
 807         reversed = qh__vec3_dot(&n, &toapex) < 0.f;
 808       }
 809     }
 810
 811     /* Create new faces */
 812     {
 813       qh_index_t top = qh__pop_stack(&context->horizonedges);
 814       qh_index_t last = qh__pop_stack(&context->horizonedges);
 815       qh_index_t first = top;
 816       int looped = 0;
 817
 818       QH_ASSERT(context->newhorizonedges.size == 0);
 819
 820       /* Release scratch */
 821       context->scratch.size = 0;
 822
 823       while (!looped) {
 824         qh_half_edge_t* prevhe;
 825         qh_half_edge_t* nexthe;
 826         qh_half_edge_t* oppedge;
 827         /* qh_vec3_t normal; */
 828         /* qh_vertex_t fcentroid; */
 829         qh_index_t verts[3];
 830         qh_face_t* newface;
 831
 832         if (last == -1) {
 833           looped = 1;
 834           last = first;
 835         }
 836
 837         prevhe = context->edges + last;
 838         nexthe = context->edges + top;
 839
 840         if (reversed) {
 841           QH_SWAP(qh_half_edge_t*, prevhe, nexthe);
 842         }
 843
 844         verts[0] = prevhe->to_vertex;
 845         verts[1] = nexthe->to_vertex;
 846         verts[2] = apex;
 847
 848         context->valid[nexthe->adjacent_face] = 0;
 849
 850         oppedge = context->edges + nexthe->opposite_he;
 851         newface = qh__next_face(context);
 852
 853         qh__face_init(newface, verts, context);
 854
 855         oppedge->opposite_he = context->edges[newface->edges[0]].he;
 856         context->edges[newface->edges[0]].opposite_he = oppedge->he;
 857
 858         qh__push_stack(&context->scratch, newface->face);
 859         qh__push_stack(&context->newhorizonedges, newface->edges[0]);
 860
 861         top = last;
 862         last = qh__pop_stack(&context->horizonedges);
 863       }
 864     }
 865
 866     /* Attach point sets to newly created faces */
 867     {
 868       for (k = 0; k < context->nfaces; ++k) {
 869         qh_face_t* f = context->faces + k;
 870
 871         if (context->valid[k] || f->iset.size == 0) {
 872           continue;
 873         }
 874
 875         if (f->visitededges == 3) {
 876           context->valid[k] = 0;
 877         }
 878
 879         for (i = 0; i < f->iset.size; ++i) {
 880           qh_index_t vertex = f->iset.indices[i];
 881           /* qh_vertex_t* v = context->vertices + vertex; */
 882           qh_face_t* dface = NULL;
 883
 884           for (j = 0; j < context->scratch.size; ++j) {
 885             qh_face_t* newface = context->faces + context->scratch.begin[j];
 886             qh_half_edge_t* e0 = context->edges + newface->edges[0];
 887             qh_half_edge_t* e1 = context->edges + newface->edges[1];
 888             qh_half_edge_t* e2 = context->edges + newface->edges[2];
 889             /* qh_vertex_t cv; */
 890
 891             if (e0->to_vertex == vertex ||
 892               e1->to_vertex == vertex ||
 893               e2->to_vertex == vertex) {
 894               continue;
 895             }
 896
 897             if (qh__face_can_see_vertex_epsilon(context, newface,
 898                                                 context->vertices + vertex,
 899                                                 epsilon)) {
 900               dface = newface;
 901               break;
 902             }
 903           }
 904
 905           if (dface) {
 906             if (dface->iset.size + 1 >= dface->iset.capacity) {
 907               dface->iset.capacity *= 2;
 908               dface->iset.indices = QH_REALLOC(qh_index_t,
 909                 dface->iset.indices, dface->iset.capacity);
 910             }
 911
 912             dface->iset.indices[dface->iset.size++] = vertex;
 913           }
 914         }
 915
 916         f->iset.size = 0;
 917       }
 918     }
 919
 920     /* Link new faces together */
 921     {
 922       for (i = 0; i < context->newhorizonedges.size; ++i) {
 923         qh_index_t phe0, nhe1;
 924         qh_half_edge_t* he0;
 925         qh_half_edge_t* he1;
 926         int ii;
 927
 928         if (reversed) {
 929           ii = (i == 0) ? context->newhorizonedges.size - 1 : (i-1);
 930         } else {
 931           ii = (i+1) % context->newhorizonedges.size;
 932         }
 933
 934         phe0 = context->edges[context->newhorizonedges.begin[i]].previous_he;
 935         nhe1 = context->edges[context->newhorizonedges.begin[ii]].next_he;
 936
 937         he0 = context->edges + phe0;
 938         he1 = context->edges + nhe1;
 939
 940         QH_ASSERT(he1->to_vertex == apex);
 941         QH_ASSERT(he0->opposite_he == -1);
 942         QH_ASSERT(he1->opposite_he == -1);
 943
 944         he0->opposite_he = he1->he;
 945         he1->opposite_he = he0->he;
 946       }
 947
 948       context->newhorizonedges.size = 0;
 949     }
 950
 951     /* Push new face to stack */
 952     {
 953       for (i = 0; i < context->scratch.size; ++i) {
 954         qh_face_t* face = context->faces + context->scratch.begin[i];
 955
 956         if (face->iset.size > 0) {
 957           qh__push_stack(&context->facestack, face->face);
 958         }
 959       }
 960
 961       /* Release scratch */
 962       context->scratch.size = 0;
 963     }
 964
 965     topface = qh__pop_stack(&context->facestack);
 966
 967     /* TODO: push all non-valid faces for reuse in face stack memory pool */
 968   }
 969 }
 970
 971 #ifdef QUICKHULL_FILES
 972 void
 973 qh_mesh_export(qh_mesh_t const* mesh, char const* filename)
 974 {
 975   FILE* objfile = fopen(filename, "wt");
 976   fprintf(objfile, "o\n");
 977
 978   for (int i = 0; i < mesh->nvertices; ++i) {
 979     qh_vertex_t v = mesh->vertices[i];
 980     fprintf(objfile, "v %f %f %f\n", v.x, v.y, v.z);
 981   }
 982
 983   for (int i = 0; i < mesh->nnormals; ++i) {
 984     qh_vec3_t n = mesh->normals[i];
 985     fprintf(objfile, "vn %f %f %f\n", n.x, n.y, n.z);
 986   }
 987
 988   for (int i = 0, j = 0; i < mesh->nindices; i += 3, j++) {
 989     fprintf(objfile, "f %u/%u %u/%u %u/%u\n",
 990       mesh->indices[i+0] + 1, mesh->normalindices[j] + 1,
 991       mesh->indices[i+1] + 1, mesh->normalindices[j] + 1,
 992       mesh->indices[i+2] + 1, mesh->normalindices[j] + 1);
 993   }
 994
 995   fclose(objfile);
 996 }
 997 #endif /* QUICKHULL_FILES */
 998
 999 static qh_face_t*
1000 qh__build_tetrahedron(qh_context_t* context, double epsilon)
1001 {
1002   int i, j;
1003   qh_index_t vertices[3];
1004   qh_index_t apex;
1005   qh_face_t* faces;
1006   qh_vertex_t normal, centroid, vapex, tcentroid;
1007
1008   /* Get the initial tetrahedron basis (first face) */
1009   qh__tetrahedron_basis(context, &vertices[0]);
1010
1011   /* Find apex from the tetrahedron basis */
1012   {
1013     double sdist;
1014     qh_vec3_t v0, v1;
1015
1016     v0 = context->vertices[vertices[1]];
1017     v1 = context->vertices[vertices[2]];
1018
1019     qh__vec3_sub(&v0, context->vertices + vertices[0]);
1020     qh__vec3_sub(&v1, context->vertices + vertices[0]);
1021
1022     normal = qh__vec3_cross(&v0, &v1);
1023     qh__vec3_normalize(&normal);
1024
1025     centroid = qh__face_centroid(vertices, context);
1026     sdist = qh__vec3_dot(&normal, &centroid);
1027
1028     apex = qh__furthest_point_from_plane(context, NULL,
1029       context->nvertices, &normal, sdist);
1030     vapex = context->vertices[apex];
1031
1032     qh__vec3_sub(&vapex, &centroid);
1033
1034     /* Whether the face is looking towards the apex */
1035     if (qh__vec3_dot(&vapex, &normal) > 0) {
1036       QH_SWAP(qh_index_t, vertices[1], vertices[2]);
1037     }
1038   }
1039
1040   faces = qh__next_face(context);
1041   qh__face_init(&faces[0], vertices, context);
1042
1043   /* Build faces from the tetrahedron basis to the apex */
1044   {
1045     qh_index_t facevertices[3];
1046     for (i = 0; i < 3; ++i) {
1047       qh_half_edge_t* edge = context->edges + faces[0].edges[i];
1048       qh_half_edge_t prevedge = context->edges[edge->previous_he];
1049       qh_face_t* face = faces+i+1;
1050       qh_half_edge_t* e0;
1051
1052       facevertices[0] = edge->to_vertex;
1053       facevertices[1] = prevedge.to_vertex;
1054       facevertices[2] = apex;
1055
1056       qh__next_face(context);
1057       qh__face_init(face, facevertices, context);
1058
1059       e0 = context->edges + faces[i+1].edges[0];
1060       edge->opposite_he = e0->he;
1061       e0->opposite_he = edge->he;
1062     }
1063   }
1064
1065   /* Attach half edges to faces tied to the apex */
1066   {
1067     for (i = 0; i < 3; ++i) {
1068       qh_face_t* face;
1069       qh_face_t* nextface;
1070       qh_half_edge_t* e1;
1071       qh_half_edge_t* e2;
1072
1073       j = (i+2) % 3;
1074
1075       face = faces+i+1;
1076       nextface = faces+j+1;
1077
1078       e1 = context->edges + face->edges[1];
1079       e2 = context->edges + nextface->edges[2];
1080
1081       QH_ASSERT(e1->opposite_he == -1);
1082       QH_ASSERT(e2->opposite_he == -1);
1083
1084       e1->opposite_he = e2->he;
1085       e2->opposite_he = e1->he;
1086
1087       qh__assert_half_edge(e1, context);
1088       qh__assert_half_edge(e2, context);
1089     }
1090   }
1091
1092   /* Create initial point set; every point is */
1093   /* attached to the first face it can see */
1094   {
1095     for (i = 0; i < context->nvertices; ++i) {
1096       /* qh_vertex_t* v; */
1097       qh_face_t* dface = NULL;
1098
1099       if (vertices[0] == i || vertices[1] == i || vertices[2] == i) {
1100         continue;
1101       }
1102
1103       for (j = 0; j < 4; ++j) {
1104         if (qh__face_can_see_vertex_epsilon(context, context->faces + j,
1105                                             context->vertices + i, epsilon)) {
1106           dface = context->faces + j;
1107           break;
1108         }
1109       }
1110
1111       if (dface) {
1112         int valid = 1;
1113         int j;
1114
1115         for (j = 0; j < 3; ++j) {
1116           qh_half_edge_t* e = context->edges + dface->edges[j];
1117           if (i == e->to_vertex) {
1118             valid = 0;
1119             break;
1120           }
1121         }
1122
1123         if (!valid) { continue; }
1124
1125         if (dface->iset.size + 1 >= dface->iset.capacity) {
1126           dface->iset.capacity *= 2;
1127           dface->iset.indices = QH_REALLOC(qh_index_t,
1128             dface->iset.indices, dface->iset.capacity);
1129         }
1130
1131         dface->iset.indices[dface->iset.size++] = i;
1132       }
1133     }
1134   }
1135
1136   /* Add initial tetrahedron faces to the face stack */
1137   tcentroid.x = tcentroid.y = tcentroid.z = 0.0;
1138   for (i = 0; i < 4; ++i) {
1139     context->valid[i] = 1;
1140     qh__assert_face(context->faces + i, context);
1141     qh__push_stack(&context->facestack, i);
1142     qh__vec3_add(&tcentroid, &context->faces[i].centroid);
1143   }
1144
1145   /* Assign the tetrahedron centroid */
1146   qh__vec3_multiply(&tcentroid, 0.25);
1147   context->centroid = tcentroid;
1148
1149   QH_ASSERT(context->nedges == context->nfaces * 3);
1150   QH_ASSERT(context->nfaces == 4);
1151   QH_ASSERT(context->facestack.size == 4);
1152
1153   return faces;
1154 }
1155
1156 static void
1157 qh__remove_vertex_duplicates(qh_context_t* context, double epsilon)
1158 {
1159   int i, j, k;
1160   for (i = 0; i < context->nvertices; ++i) {
1161     qh_vertex_t* v = context->vertices + i;
1162     if (v->x == 0) v->x = 0;
1163     if (v->y == 0) v->y = 0;
1164     if (v->z == 0) v->z = 0;
1165     for (j = i + 1; j < context->nvertices; ++j) {
1166       if (qh__vertex_equals_epsilon(context->vertices + i,
1167             context->vertices + j, epsilon))
1168       {
1169         for (k = j; k < context->nvertices - 1; ++k) {
1170           context->vertices[k] = context->vertices[k+1];
1171         }
1172         context->nvertices--;
1173       }
1174     }
1175   }
1176 }
1177
1178 static void
1179 qh__init_context(qh_context_t* context, qh_vertex_t const* vertices,
1180                  unsigned int nvertices)
1181 {
1182   /* TODO: */
1183   /* size_t nedges = 3 * nvertices - 6; */
1184   /* size_t nfaces = 2 * nvertices - 4; */
1185   unsigned int nfaces = nvertices * (nvertices - 1);
1186   unsigned int nedges = nfaces * 3;
1187
1188   context->edges = QH_MALLOC(qh_half_edge_t, nedges);
1189   context->faces = QH_MALLOC(qh_face_t, nfaces);
1190   context->facestack.begin = QH_MALLOC(qh_index_t, nfaces);
1191   context->scratch.begin = QH_MALLOC(qh_index_t, nfaces);
1192   context->horizonedges.begin = QH_MALLOC(qh_index_t, nedges);
1193   context->newhorizonedges.begin = QH_MALLOC(qh_index_t, nedges);
1194   context->valid = QH_MALLOC(char, nfaces);
1195
1196   if (!(context->edges &&
1197         context->faces &&
1198         context->facestack.begin &&
1199         context->scratch.begin &&
1200         context->horizonedges.begin &&
1201         context->newhorizonedges.begin &&
1202         context->valid)) {
1203 # ifdef HAVE_JWXYZ
1204     jwxyz_abort ("%s: out of memory", progname);
1205 # else
1206     fprintf (stderr, "%s: out of memory\n", progname);
1207     exit (1);
1208 # endif
1209   }
1210
1211
1212   context->vertices = QH_MALLOC(qh_vertex_t, nvertices);
1213   memcpy(context->vertices, vertices, sizeof(qh_vertex_t) * nvertices);
1214
1215   context->nvertices = nvertices;
1216   context->nedges = 0;
1217   context->nfaces = 0;
1218   context->facestack.size = 0;
1219   context->scratch.size = 0;
1220   context->horizonedges.size = 0;
1221   context->newhorizonedges.size = 0;
1222
1223   #ifdef QUICKHULL_DEBUG
1224   context->maxfaces = nfaces;
1225   context->maxedges = nedges;
1226   #endif
1227 }
1228
1229 static void
1230 qh__free_context(qh_context_t* context)
1231 {
1232   int i;
1233
1234   for (i = 0; i < context->nfaces; ++i) {
1235     QH_FREE(context->faces[i].iset.indices);
1236     context->faces[i].iset.size = 0;
1237   }
1238
1239   context->nvertices = 0;
1240   context->nfaces = 0;
1241
1242   QH_FREE(context->edges);
1243
1244   QH_FREE(context->faces);
1245   QH_FREE(context->facestack.begin);
1246   QH_FREE(context->scratch.begin);
1247   QH_FREE(context->horizonedges.begin);
1248   QH_FREE(context->newhorizonedges.begin);
1249   QH_FREE(context->vertices);
1250   QH_FREE(context->valid);
1251 }
1252
1253 void
1254 qh_free_mesh(qh_mesh_t mesh)
1255 {
1256   QH_FREE(mesh.vertices);
1257   QH_FREE(mesh.indices);
1258   QH_FREE(mesh.normalindices);
1259   QH_FREE(mesh.normals);
1260 }
1261
1262 static double
1263 qh__compute_epsilon(qh_vertex_t const* vertices, unsigned int nvertices)
1264 {
1265   double epsilon;
1266   unsigned int i;
1267
1268   double maxxi = -QH_FLT_MAX;
1269   double maxyi = -QH_FLT_MAX;
1270
1271   for (i = 0; i < nvertices; ++i) {
1272     double fxi = fabs(vertices[i].x);
1273     double fyi = fabs(vertices[i].y);
1274
1275     if (fxi > maxxi) {
1276       maxxi = fxi;
1277     }
1278     if (fyi > maxyi) {
1279       maxyi = fyi;
1280     }
1281   }
1282
1283   epsilon = 2 * (maxxi + maxyi) * QH_FLT_EPS;
1284
1285   return epsilon;
1286 }
1287
1288 qh_mesh_t
1289 qh_quickhull3d(qh_vertex_t const* vertices, unsigned int nvertices)
1290 {
1291   qh_mesh_t m;
1292   qh_context_t context;
1293   /* unsigned int* indices; */
1294   unsigned int nfaces = 0, i, index, nindices;
1295   double epsilon;
1296
1297   epsilon = qh__compute_epsilon(vertices, nvertices);
1298
1299   qh__init_context(&context, vertices, nvertices);
1300
1301   qh__remove_vertex_duplicates(&context, epsilon);
1302
1303   /* Build the initial tetrahedron */
1304   qh__build_tetrahedron(&context, epsilon);
1305
1306   /* Build the convex hull */
1307   #ifdef QUICKHULL_DEBUG
1308   qh__build_hull(&context, epsilon, -1, NULL);
1309   #else
1310   qh__build_hull(&context, epsilon);
1311   #endif
1312
1313   /* QH_ASSERT(qh__test_hull(&context, epsilon)); */
1314
1315   for (i = 0; i < context.nfaces; ++i) {
1316     if (context.valid[i]) { nfaces++; }
1317   }
1318
1319   nindices = nfaces * 3;
1320
1321   m.normals = QH_MALLOC(qh_vertex_t, nfaces);
1322   m.normalindices = QH_MALLOC(unsigned int, nfaces);
1323   m.vertices = QH_MALLOC(qh_vertex_t, nindices);
1324   m.indices = QH_MALLOC(unsigned int, nindices);
1325   m.nindices = nindices;
1326   m.nnormals = nfaces;
1327   m.nvertices = 0;
1328
1329   {
1330     index = 0;
1331     for (i = 0; i < context.nfaces; ++i) {
1332       if (!context.valid[i]) { continue; }
1333       m.normals[index] = context.faces[i].normal;
1334       index++;
1335     }
1336
1337     index = 0;
1338     for (i = 0; i < context.nfaces; ++i) {
1339       if (!context.valid[i]) { continue; }
1340       m.normalindices[index] = index;
1341       index++;
1342     }
1343
1344     index = 0;
1345     for (i = 0; i < context.nfaces; ++i) {
1346       if (!context.valid[i]) { continue; }
1347       m.indices[index+0] = index+0;
1348       m.indices[index+1] = index+1;
1349       m.indices[index+2] = index+2;
1350       index += 3;
1351     }
1352
1353     for (i = 0; i < context.nfaces; ++i) {
1354       if (!context.valid[i]) { continue; }
1355       qh_half_edge_t e0 = context.edges[context.faces[i].edges[0]];
1356       qh_half_edge_t e1 = context.edges[context.faces[i].edges[1]];
1357       qh_half_edge_t e2 = context.edges[context.faces[i].edges[2]];
1358
1359       m.vertices[m.nvertices++] = context.vertices[e0.to_vertex];
1360       m.vertices[m.nvertices++] = context.vertices[e1.to_vertex];
1361       m.vertices[m.nvertices++] = context.vertices[e2.to_vertex];
1362     }
1363   }
1364
1365   qh__free_context(&context);
1366
1367   return m;
1368 }