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