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