1 /* 2 Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net 3 4 This software is provided 'as-is', without any express or implied warranty. 5 In no event will the authors be held liable for any damages arising from the use of this software. 6 Permission is granted to anyone to use this software for any purpose, 7 including commercial applications, and to alter it and redistribute it freely, 8 subject to the following restrictions: 9 10 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 12 3. This notice may not be removed or altered from any source distribution. 13 */ 14 15 #include <string.h> 16 17 #include "btConvexHullComputer.h" 18 #include "btAlignedObjectArray.h" 19 #include "btMinMax.h" 20 #include "btVector3.h" 21 22 #ifdef __GNUC__ 23 #include <stdint.h> 24 #elif defined(_MSC_VER) 25 typedef __int32 int32_t; 26 typedef __int64 int64_t; 27 typedef unsigned __int32 uint32_t; 28 typedef unsigned __int64 uint64_t; 29 #else 30 typedef int int32_t; 31 typedef long long int int64_t; 32 typedef unsigned int uint32_t; 33 typedef unsigned long long int uint64_t; 34 #endif 35 36 37 //The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines 38 //#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL)) // || (defined(__ICL) && defined(_M_X64)) bug in Intel compiler, disable inline assembly 39 // #define USE_X86_64_ASM 40 //#endif 41 42 43 //#define DEBUG_CONVEX_HULL 44 //#define SHOW_ITERATIONS 45 46 #if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS) 47 #include <stdio.h> 48 #endif 49 50 // Convex hull implementation based on Preparata and Hong 51 // Ole Kniemeyer, MAXON Computer GmbH 52 class btConvexHullInternal 53 { 54 public: 55 56 class Point64 57 { 58 public: 59 int64_t x; 60 int64_t y; 61 int64_t z; 62 63 Point64(int64_t x, int64_t y, int64_t z): x(x), y(y), z(z) 64 { 65 } 66 67 bool isZero() 68 { 69 return (x == 0) && (y == 0) && (z == 0); 70 } 71 72 int64_t dot(const Point64& b) const 73 { 74 return x * b.x + y * b.y + z * b.z; 75 } 76 }; 77 78 class Point32 79 { 80 public: 81 int32_t x; 82 int32_t y; 83 int32_t z; 84 int index; 85 86 Point32() 87 { 88 } 89 90 Point32(int32_t x, int32_t y, int32_t z): x(x), y(y), z(z), index(-1) 91 { 92 } 93 94 bool operator==(const Point32& b) const 95 { 96 return (x == b.x) && (y == b.y) && (z == b.z); 97 } 98 99 bool operator!=(const Point32& b) const 100 { 101 return (x != b.x) || (y != b.y) || (z != b.z); 102 } 103 104 bool isZero() 105 { 106 return (x == 0) && (y == 0) && (z == 0); 107 } 108 109 Point64 cross(const Point32& b) const 110 { 111 return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); 112 } 113 114 Point64 cross(const Point64& b) const 115 { 116 return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); 117 } 118 119 int64_t dot(const Point32& b) const 120 { 121 return x * b.x + y * b.y + z * b.z; 122 } 123 124 int64_t dot(const Point64& b) const 125 { 126 return x * b.x + y * b.y + z * b.z; 127 } 128 129 Point32 operator+(const Point32& b) const 130 { 131 return Point32(x + b.x, y + b.y, z + b.z); 132 } 133 134 Point32 operator-(const Point32& b) const 135 { 136 return Point32(x - b.x, y - b.y, z - b.z); 137 } 138 }; 139 140 class Int128 141 { 142 public: 143 uint64_t low; 144 uint64_t high; 145 146 Int128() 147 { 148 } 149 150 Int128(uint64_t low, uint64_t high): low(low), high(high) 151 { 152 } 153 154 Int128(uint64_t low): low(low), high(0) 155 { 156 } 157 158 Int128(int64_t value): low(value), high((value >= 0) ? 0 : (uint64_t) -1LL) 159 { 160 } 161 162 static Int128 mul(int64_t a, int64_t b); 163 164 static Int128 mul(uint64_t a, uint64_t b); 165 166 Int128 operator-() const 167 { 168 return Int128((uint64_t) -(int64_t)low, ~high + (low == 0)); 169 } 170 171 Int128 operator+(const Int128& b) const 172 { 173 #ifdef USE_X86_64_ASM 174 Int128 result; 175 __asm__ ("addq %[bl], %[rl]\n\t" 176 "adcq %[bh], %[rh]\n\t" 177 : [rl] "=r" (result.low), [rh] "=r" (result.high) 178 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) 179 : "cc" ); 180 return result; 181 #else 182 uint64_t lo = low + b.low; 183 return Int128(lo, high + b.high + (lo < low)); 184 #endif 185 } 186 187 Int128 operator-(const Int128& b) const 188 { 189 #ifdef USE_X86_64_ASM 190 Int128 result; 191 __asm__ ("subq %[bl], %[rl]\n\t" 192 "sbbq %[bh], %[rh]\n\t" 193 : [rl] "=r" (result.low), [rh] "=r" (result.high) 194 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) 195 : "cc" ); 196 return result; 197 #else 198 return *this + -b; 199 #endif 200 } 201 202 Int128& operator+=(const Int128& b) 203 { 204 #ifdef USE_X86_64_ASM 205 __asm__ ("addq %[bl], %[rl]\n\t" 206 "adcq %[bh], %[rh]\n\t" 207 : [rl] "=r" (low), [rh] "=r" (high) 208 : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) 209 : "cc" ); 210 #else 211 uint64_t lo = low + b.low; 212 if (lo < low) 213 { 214 ++high; 215 } 216 low = lo; 217 high += b.high; 218 #endif 219 return *this; 220 } 221 222 Int128& operator++() 223 { 224 if (++low == 0) 225 { 226 ++high; 227 } 228 return *this; 229 } 230 231 Int128 operator*(int64_t b) const; 232 233 btScalar toScalar() const 234 { 235 return ((int64_t) high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low) 236 : -(-*this).toScalar(); 237 } 238 239 int getSign() const 240 { 241 return ((int64_t) high < 0) ? -1 : (high || low) ? 1 : 0; 242 } 243 244 bool operator<(const Int128& b) const 245 { 246 return (high < b.high) || ((high == b.high) && (low < b.low)); 247 } 248 249 int ucmp(const Int128&b) const 250 { 251 if (high < b.high) 252 { 253 return -1; 254 } 255 if (high > b.high) 256 { 257 return 1; 258 } 259 if (low < b.low) 260 { 261 return -1; 262 } 263 if (low > b.low) 264 { 265 return 1; 266 } 267 return 0; 268 } 269 }; 270 271 272 class Rational64 273 { 274 private: 275 uint64_t m_numerator; 276 uint64_t m_denominator; 277 int sign; 278 279 public: 280 Rational64(int64_t numerator, int64_t denominator) 281 { 282 if (numerator > 0) 283 { 284 sign = 1; 285 m_numerator = (uint64_t) numerator; 286 } 287 else if (numerator < 0) 288 { 289 sign = -1; 290 m_numerator = (uint64_t) -numerator; 291 } 292 else 293 { 294 sign = 0; 295 m_numerator = 0; 296 } 297 if (denominator > 0) 298 { 299 m_denominator = (uint64_t) denominator; 300 } 301 else if (denominator < 0) 302 { 303 sign = -sign; 304 m_denominator = (uint64_t) -denominator; 305 } 306 else 307 { 308 m_denominator = 0; 309 } 310 } 311 312 bool isNegativeInfinity() const 313 { 314 return (sign < 0) && (m_denominator == 0); 315 } 316 317 bool isNaN() const 318 { 319 return (sign == 0) && (m_denominator == 0); 320 } 321 322 int compare(const Rational64& b) const; 323 324 btScalar toScalar() const 325 { 326 return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar) m_numerator / m_denominator); 327 } 328 }; 329 330 331 class Rational128 332 { 333 private: 334 Int128 numerator; 335 Int128 denominator; 336 int sign; 337 bool isInt64; 338 339 public: 340 Rational128(int64_t value) 341 { 342 if (value > 0) 343 { 344 sign = 1; 345 this->numerator = value; 346 } 347 else if (value < 0) 348 { 349 sign = -1; 350 this->numerator = -value; 351 } 352 else 353 { 354 sign = 0; 355 this->numerator = (uint64_t) 0; 356 } 357 this->denominator = (uint64_t) 1; 358 isInt64 = true; 359 } 360 361 Rational128(const Int128& numerator, const Int128& denominator) 362 { 363 sign = numerator.getSign(); 364 if (sign >= 0) 365 { 366 this->numerator = numerator; 367 } 368 else 369 { 370 this->numerator = -numerator; 371 } 372 int dsign = denominator.getSign(); 373 if (dsign >= 0) 374 { 375 this->denominator = denominator; 376 } 377 else 378 { 379 sign = -sign; 380 this->denominator = -denominator; 381 } 382 isInt64 = false; 383 } 384 385 int compare(const Rational128& b) const; 386 387 int compare(int64_t b) const; 388 389 btScalar toScalar() const 390 { 391 return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar()); 392 } 393 }; 394 395 class PointR128 396 { 397 public: 398 Int128 x; 399 Int128 y; 400 Int128 z; 401 Int128 denominator; 402 403 PointR128() 404 { 405 } 406 407 PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator): x(x), y(y), z(z), denominator(denominator) 408 { 409 } 410 411 btScalar xvalue() const 412 { 413 return x.toScalar() / denominator.toScalar(); 414 } 415 416 btScalar yvalue() const 417 { 418 return y.toScalar() / denominator.toScalar(); 419 } 420 421 btScalar zvalue() const 422 { 423 return z.toScalar() / denominator.toScalar(); 424 } 425 }; 426 427 428 class Edge; 429 class Face; 430 431 class Vertex 432 { 433 public: 434 Vertex* next; 435 Vertex* prev; 436 Edge* edges; 437 Face* firstNearbyFace; 438 Face* lastNearbyFace; 439 PointR128 point128; 440 Point32 point; 441 int copy; 442 443 Vertex(): next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1) 444 { 445 } 446 447 #ifdef DEBUG_CONVEX_HULL 448 void print() 449 { 450 printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z); 451 } 452 453 void printGraph(); 454 #endif 455 456 Point32 operator-(const Vertex& b) const 457 { 458 return point - b.point; 459 } 460 461 Rational128 dot(const Point64& b) const 462 { 463 return (point.index >= 0) ? Rational128(point.dot(b)) 464 : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator); 465 } 466 467 btScalar xvalue() const 468 { 469 return (point.index >= 0) ? btScalar(point.x) : point128.xvalue(); 470 } 471 472 btScalar yvalue() const 473 { 474 return (point.index >= 0) ? btScalar(point.y) : point128.yvalue(); 475 } 476 477 btScalar zvalue() const 478 { 479 return (point.index >= 0) ? btScalar(point.z) : point128.zvalue(); 480 } 481 482 void receiveNearbyFaces(Vertex* src) 483 { 484 if (lastNearbyFace) 485 { 486 lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace; 487 } 488 else 489 { 490 firstNearbyFace = src->firstNearbyFace; 491 } 492 if (src->lastNearbyFace) 493 { 494 lastNearbyFace = src->lastNearbyFace; 495 } 496 for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex) 497 { 498 btAssert(f->nearbyVertex == src); 499 f->nearbyVertex = this; 500 } 501 src->firstNearbyFace = NULL; 502 src->lastNearbyFace = NULL; 503 } 504 }; 505 506 507 class Edge 508 { 509 public: 510 Edge* next; 511 Edge* prev; 512 Edge* reverse; 513 Vertex* target; 514 Face* face; 515 int copy; 516 517 ~Edge() 518 { 519 next = NULL; 520 prev = NULL; 521 reverse = NULL; 522 target = NULL; 523 face = NULL; 524 } 525 526 void link(Edge* n) 527 { 528 btAssert(reverse->target == n->reverse->target); 529 next = n; 530 n->prev = this; 531 } 532 533 #ifdef DEBUG_CONVEX_HULL 534 void print() 535 { 536 printf("E%p : %d -> %d, n=%p p=%p (0 %d\t%d\t%d) -> (%d %d %d)", this, reverse->target->point.index, target->point.index, next, prev, 537 reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z); 538 } 539 #endif 540 }; 541 542 class Face 543 { 544 public: 545 Face* next; 546 Vertex* nearbyVertex; 547 Face* nextWithSameNearbyVertex; 548 Point32 origin; 549 Point32 dir0; 550 Point32 dir1; 551 552 Face(): next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL) 553 { 554 } 555 556 void init(Vertex* a, Vertex* b, Vertex* c) 557 { 558 nearbyVertex = a; 559 origin = a->point; 560 dir0 = *b - *a; 561 dir1 = *c - *a; 562 if (a->lastNearbyFace) 563 { 564 a->lastNearbyFace->nextWithSameNearbyVertex = this; 565 } 566 else 567 { 568 a->firstNearbyFace = this; 569 } 570 a->lastNearbyFace = this; 571 } 572 573 Point64 getNormal() 574 { 575 return dir0.cross(dir1); 576 } 577 }; 578 579 template<typename UWord, typename UHWord> class DMul 580 { 581 private: 582 static uint32_t high(uint64_t value) 583 { 584 return (uint32_t) (value >> 32); 585 } 586 587 static uint32_t low(uint64_t value) 588 { 589 return (uint32_t) value; 590 } 591 592 static uint64_t mul(uint32_t a, uint32_t b) 593 { 594 return (uint64_t) a * (uint64_t) b; 595 } 596 597 static void shlHalf(uint64_t& value) 598 { 599 value <<= 32; 600 } 601 602 static uint64_t high(Int128 value) 603 { 604 return value.high; 605 } 606 607 static uint64_t low(Int128 value) 608 { 609 return value.low; 610 } 611 612 static Int128 mul(uint64_t a, uint64_t b) 613 { 614 return Int128::mul(a, b); 615 } 616 617 static void shlHalf(Int128& value) 618 { 619 value.high = value.low; 620 value.low = 0; 621 } 622 623 public: 624 625 static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh) 626 { 627 UWord p00 = mul(low(a), low(b)); 628 UWord p01 = mul(low(a), high(b)); 629 UWord p10 = mul(high(a), low(b)); 630 UWord p11 = mul(high(a), high(b)); 631 UWord p0110 = UWord(low(p01)) + UWord(low(p10)); 632 p11 += high(p01); 633 p11 += high(p10); 634 p11 += high(p0110); 635 shlHalf(p0110); 636 p00 += p0110; 637 if (p00 < p0110) 638 { 639 ++p11; 640 } 641 resLow = p00; 642 resHigh = p11; 643 } 644 }; 645 646 private: 647 648 class IntermediateHull 649 { 650 public: 651 Vertex* minXy; 652 Vertex* maxXy; 653 Vertex* minYx; 654 Vertex* maxYx; 655 656 IntermediateHull(): minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL) 657 { 658 } 659 660 void print(); 661 }; 662 663 enum Orientation {NONE, CLOCKWISE, COUNTER_CLOCKWISE}; 664 665 template <typename T> class PoolArray 666 { 667 private: 668 T* array; 669 int size; 670 671 public: 672 PoolArray<T>* next; 673 674 PoolArray(int size): size(size), next(NULL) 675 { 676 array = (T*) btAlignedAlloc(sizeof(T) * size, 16); 677 } 678 679 ~PoolArray() 680 { 681 btAlignedFree(array); 682 } 683 684 T* init() 685 { 686 T* o = array; 687 for (int i = 0; i < size; i++, o++) 688 { 689 o->next = (i+1 < size) ? o + 1 : NULL; 690 } 691 return array; 692 } 693 }; 694 695 template <typename T> class Pool 696 { 697 private: 698 PoolArray<T>* arrays; 699 PoolArray<T>* nextArray; 700 T* freeObjects; 701 int arraySize; 702 703 public: 704 Pool(): arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256) 705 { 706 } 707 708 ~Pool() 709 { 710 while (arrays) 711 { 712 PoolArray<T>* p = arrays; 713 arrays = p->next; 714 p->~PoolArray<T>(); 715 btAlignedFree(p); 716 } 717 } 718 719 void reset() 720 { 721 nextArray = arrays; 722 freeObjects = NULL; 723 } 724 725 void setArraySize(int arraySize) 726 { 727 this->arraySize = arraySize; 728 } 729 730 T* newObject() 731 { 732 T* o = freeObjects; 733 if (!o) 734 { 735 PoolArray<T>* p = nextArray; 736 if (p) 737 { 738 nextArray = p->next; 739 } 740 else 741 { 742 p = new(btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize); 743 p->next = arrays; 744 arrays = p; 745 } 746 o = p->init(); 747 } 748 freeObjects = o->next; 749 return new(o) T(); 750 }; 751 752 void freeObject(T* object) 753 { 754 object->~T(); 755 object->next = freeObjects; 756 freeObjects = object; 757 } 758 }; 759 760 btVector3 scaling; 761 btVector3 center; 762 Pool<Vertex> vertexPool; 763 Pool<Edge> edgePool; 764 Pool<Face> facePool; 765 btAlignedObjectArray<Vertex*> originalVertices; 766 int mergeStamp; 767 int minAxis; 768 int medAxis; 769 int maxAxis; 770 int usedEdgePairs; 771 int maxUsedEdgePairs; 772 773 static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t); 774 Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot); 775 void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1); 776 777 Edge* newEdgePair(Vertex* from, Vertex* to); 778 779 void removeEdgePair(Edge* edge) 780 { 781 Edge* n = edge->next; 782 Edge* r = edge->reverse; 783 784 btAssert(edge->target && r->target); 785 786 if (n != edge) 787 { 788 n->prev = edge->prev; 789 edge->prev->next = n; 790 r->target->edges = n; 791 } 792 else 793 { 794 r->target->edges = NULL; 795 } 796 797 n = r->next; 798 799 if (n != r) 800 { 801 n->prev = r->prev; 802 r->prev->next = n; 803 edge->target->edges = n; 804 } 805 else 806 { 807 edge->target->edges = NULL; 808 } 809 810 edgePool.freeObject(edge); 811 edgePool.freeObject(r); 812 usedEdgePairs--; 813 } 814 815 void computeInternal(int start, int end, IntermediateHull& result); 816 817 bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1); 818 819 void merge(IntermediateHull& h0, IntermediateHull& h1); 820 821 btVector3 toBtVector(const Point32& v); 822 823 btVector3 getBtNormal(Face* face); 824 825 bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack); 826 827 public: 828 Vertex* vertexList; 829 830 void compute(const void* coords, bool doubleCoords, int stride, int count); 831 832 btVector3 getCoordinates(const Vertex* v); 833 834 btScalar shrink(btScalar amount, btScalar clampAmount); 835 }; 836 837 838 btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const 839 { 840 bool negative = (int64_t) high < 0; 841 Int128 a = negative ? -*this : *this; 842 if (b < 0) 843 { 844 negative = !negative; 845 b = -b; 846 } 847 Int128 result = mul(a.low, (uint64_t) b); 848 result.high += a.high * (uint64_t) b; 849 return negative ? -result : result; 850 } 851 852 btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b) 853 { 854 Int128 result; 855 856 #ifdef USE_X86_64_ASM 857 __asm__ ("imulq %[b]" 858 : "=a" (result.low), "=d" (result.high) 859 : "0"(a), [b] "r"(b) 860 : "cc" ); 861 return result; 862 863 #else 864 bool negative = a < 0; 865 if (negative) 866 { 867 a = -a; 868 } 869 if (b < 0) 870 { 871 negative = !negative; 872 b = -b; 873 } 874 DMul<uint64_t, uint32_t>::mul((uint64_t) a, (uint64_t) b, result.low, result.high); 875 return negative ? -result : result; 876 #endif 877 } 878 879 btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_t b) 880 { 881 Int128 result; 882 883 #ifdef USE_X86_64_ASM 884 __asm__ ("mulq %[b]" 885 : "=a" (result.low), "=d" (result.high) 886 : "0"(a), [b] "r"(b) 887 : "cc" ); 888 889 #else 890 DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high); 891 #endif 892 893 return result; 894 } 895 896 int btConvexHullInternal::Rational64::compare(const Rational64& b) const 897 { 898 if (sign != b.sign) 899 { 900 return sign - b.sign; 901 } 902 else if (sign == 0) 903 { 904 return 0; 905 } 906 907 // return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0; 908 909 #ifdef USE_X86_64_ASM 910 911 int result; 912 int64_t tmp; 913 int64_t dummy; 914 __asm__ ("mulq %[bn]\n\t" 915 "movq %%rax, %[tmp]\n\t" 916 "movq %%rdx, %%rbx\n\t" 917 "movq %[tn], %%rax\n\t" 918 "mulq %[bd]\n\t" 919 "subq %[tmp], %%rax\n\t" 920 "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator" 921 "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise 922 "orq %%rdx, %%rax\n\t" 923 "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero 924 "decb %%bh\n\t" // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference) 925 "shll $16, %%ebx\n\t" // ebx has same sign as difference 926 : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy) 927 : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator) 928 : "%rdx", "cc" ); 929 return result ? result ^ sign // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero) 930 // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero) 931 : 0; 932 933 #else 934 935 return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator)); 936 937 #endif 938 } 939 940 int btConvexHullInternal::Rational128::compare(const Rational128& b) const 941 { 942 if (sign != b.sign) 943 { 944 return sign - b.sign; 945 } 946 else if (sign == 0) 947 { 948 return 0; 949 } 950 if (isInt64) 951 { 952 return -b.compare(sign * (int64_t) numerator.low); 953 } 954 955 Int128 nbdLow, nbdHigh, dbnLow, dbnHigh; 956 DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh); 957 DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh); 958 959 int cmp = nbdHigh.ucmp(dbnHigh); 960 if (cmp) 961 { 962 return cmp * sign; 963 } 964 return nbdLow.ucmp(dbnLow) * sign; 965 } 966 967 int btConvexHullInternal::Rational128::compare(int64_t b) const 968 { 969 if (isInt64) 970 { 971 int64_t a = sign * (int64_t) numerator.low; 972 return (a > b) ? 1 : (a < b) ? -1 : 0; 973 } 974 if (b > 0) 975 { 976 if (sign <= 0) 977 { 978 return -1; 979 } 980 } 981 else if (b < 0) 982 { 983 if (sign >= 0) 984 { 985 return 1; 986 } 987 b = -b; 988 } 989 else 990 { 991 return sign; 992 } 993 994 return numerator.ucmp(denominator * b) * sign; 995 } 996 997 998 btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to) 999 { 1000 btAssert(from && to); 1001 Edge* e = edgePool.newObject(); 1002 Edge* r = edgePool.newObject(); 1003 e->reverse = r; 1004 r->reverse = e; 1005 e->copy = mergeStamp; 1006 r->copy = mergeStamp; 1007 e->target = to; 1008 r->target = from; 1009 e->face = NULL; 1010 r->face = NULL; 1011 usedEdgePairs++; 1012 if (usedEdgePairs > maxUsedEdgePairs) 1013 { 1014 maxUsedEdgePairs = usedEdgePairs; 1015 } 1016 return e; 1017 } 1018 1019 bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1) 1020 { 1021 Vertex* v0 = h0.maxYx; 1022 Vertex* v1 = h1.minYx; 1023 if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y)) 1024 { 1025 btAssert(v0->point.z < v1->point.z); 1026 Vertex* v1p = v1->prev; 1027 if (v1p == v1) 1028 { 1029 c0 = v0; 1030 if (v1->edges) 1031 { 1032 btAssert(v1->edges->next == v1->edges); 1033 v1 = v1->edges->target; 1034 btAssert(v1->edges->next == v1->edges); 1035 } 1036 c1 = v1; 1037 return false; 1038 } 1039 Vertex* v1n = v1->next; 1040 v1p->next = v1n; 1041 v1n->prev = v1p; 1042 if (v1 == h1.minXy) 1043 { 1044 if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y))) 1045 { 1046 h1.minXy = v1n; 1047 } 1048 else 1049 { 1050 h1.minXy = v1p; 1051 } 1052 } 1053 if (v1 == h1.maxXy) 1054 { 1055 if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y))) 1056 { 1057 h1.maxXy = v1n; 1058 } 1059 else 1060 { 1061 h1.maxXy = v1p; 1062 } 1063 } 1064 } 1065 1066 v0 = h0.maxXy; 1067 v1 = h1.maxXy; 1068 Vertex* v00 = NULL; 1069 Vertex* v10 = NULL; 1070 int32_t sign = 1; 1071 1072 for (int side = 0; side <= 1; side++) 1073 { 1074 int32_t dx = (v1->point.x - v0->point.x) * sign; 1075 if (dx > 0) 1076 { 1077 while (true) 1078 { 1079 int32_t dy = v1->point.y - v0->point.y; 1080 1081 Vertex* w0 = side ? v0->next : v0->prev; 1082 if (w0 != v0) 1083 { 1084 int32_t dx0 = (w0->point.x - v0->point.x) * sign; 1085 int32_t dy0 = w0->point.y - v0->point.y; 1086 if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0)))) 1087 { 1088 v0 = w0; 1089 dx = (v1->point.x - v0->point.x) * sign; 1090 continue; 1091 } 1092 } 1093 1094 Vertex* w1 = side ? v1->next : v1->prev; 1095 if (w1 != v1) 1096 { 1097 int32_t dx1 = (w1->point.x - v1->point.x) * sign; 1098 int32_t dy1 = w1->point.y - v1->point.y; 1099 int32_t dxn = (w1->point.x - v0->point.x) * sign; 1100 if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1)))) 1101 { 1102 v1 = w1; 1103 dx = dxn; 1104 continue; 1105 } 1106 } 1107 1108 break; 1109 } 1110 } 1111 else if (dx < 0) 1112 { 1113 while (true) 1114 { 1115 int32_t dy = v1->point.y - v0->point.y; 1116 1117 Vertex* w1 = side ? v1->prev : v1->next; 1118 if (w1 != v1) 1119 { 1120 int32_t dx1 = (w1->point.x - v1->point.x) * sign; 1121 int32_t dy1 = w1->point.y - v1->point.y; 1122 if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1)))) 1123 { 1124 v1 = w1; 1125 dx = (v1->point.x - v0->point.x) * sign; 1126 continue; 1127 } 1128 } 1129 1130 Vertex* w0 = side ? v0->prev : v0->next; 1131 if (w0 != v0) 1132 { 1133 int32_t dx0 = (w0->point.x - v0->point.x) * sign; 1134 int32_t dy0 = w0->point.y - v0->point.y; 1135 int32_t dxn = (v1->point.x - w0->point.x) * sign; 1136 if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0)))) 1137 { 1138 v0 = w0; 1139 dx = dxn; 1140 continue; 1141 } 1142 } 1143 1144 break; 1145 } 1146 } 1147 else 1148 { 1149 int32_t x = v0->point.x; 1150 int32_t y0 = v0->point.y; 1151 Vertex* w0 = v0; 1152 Vertex* t; 1153 while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0)) 1154 { 1155 w0 = t; 1156 y0 = t->point.y; 1157 } 1158 v0 = w0; 1159 1160 int32_t y1 = v1->point.y; 1161 Vertex* w1 = v1; 1162 while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1)) 1163 { 1164 w1 = t; 1165 y1 = t->point.y; 1166 } 1167 v1 = w1; 1168 } 1169 1170 if (side == 0) 1171 { 1172 v00 = v0; 1173 v10 = v1; 1174 1175 v0 = h0.minXy; 1176 v1 = h1.minXy; 1177 sign = -1; 1178 } 1179 } 1180 1181 v0->prev = v1; 1182 v1->next = v0; 1183 1184 v00->next = v10; 1185 v10->prev = v00; 1186 1187 if (h1.minXy->point.x < h0.minXy->point.x) 1188 { 1189 h0.minXy = h1.minXy; 1190 } 1191 if (h1.maxXy->point.x >= h0.maxXy->point.x) 1192 { 1193 h0.maxXy = h1.maxXy; 1194 } 1195 1196 h0.maxYx = h1.maxYx; 1197 1198 c0 = v00; 1199 c1 = v10; 1200 1201 return true; 1202 } 1203 1204 void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull& result) 1205 { 1206 int n = end - start; 1207 switch (n) 1208 { 1209 case 0: 1210 result.minXy = NULL; 1211 result.maxXy = NULL; 1212 result.minYx = NULL; 1213 result.maxYx = NULL; 1214 return; 1215 case 2: 1216 { 1217 Vertex* v = originalVertices[start]; 1218 Vertex* w = v + 1; 1219 if (v->point != w->point) 1220 { 1221 int32_t dx = v->point.x - w->point.x; 1222 int32_t dy = v->point.y - w->point.y; 1223 1224 if ((dx == 0) && (dy == 0)) 1225 { 1226 if (v->point.z > w->point.z) 1227 { 1228 Vertex* t = w; 1229 w = v; 1230 v = t; 1231 } 1232 btAssert(v->point.z < w->point.z); 1233 v->next = v; 1234 v->prev = v; 1235 result.minXy = v; 1236 result.maxXy = v; 1237 result.minYx = v; 1238 result.maxYx = v; 1239 } 1240 else 1241 { 1242 v->next = w; 1243 v->prev = w; 1244 w->next = v; 1245 w->prev = v; 1246 1247 if ((dx < 0) || ((dx == 0) && (dy < 0))) 1248 { 1249 result.minXy = v; 1250 result.maxXy = w; 1251 } 1252 else 1253 { 1254 result.minXy = w; 1255 result.maxXy = v; 1256 } 1257 1258 if ((dy < 0) || ((dy == 0) && (dx < 0))) 1259 { 1260 result.minYx = v; 1261 result.maxYx = w; 1262 } 1263 else 1264 { 1265 result.minYx = w; 1266 result.maxYx = v; 1267 } 1268 } 1269 1270 Edge* e = newEdgePair(v, w); 1271 e->link(e); 1272 v->edges = e; 1273 1274 e = e->reverse; 1275 e->link(e); 1276 w->edges = e; 1277 1278 return; 1279 } 1280 } 1281 // lint -fallthrough 1282 case 1: 1283 { 1284 Vertex* v = originalVertices[start]; 1285 v->edges = NULL; 1286 v->next = v; 1287 v->prev = v; 1288 1289 result.minXy = v; 1290 result.maxXy = v; 1291 result.minYx = v; 1292 result.maxYx = v; 1293 1294 return; 1295 } 1296 } 1297 1298 int split0 = start + n / 2; 1299 Point32 p = originalVertices[split0-1]->point; 1300 int split1 = split0; 1301 while ((split1 < end) && (originalVertices[split1]->point == p)) 1302 { 1303 split1++; 1304 } 1305 computeInternal(start, split0, result); 1306 IntermediateHull hull1; 1307 computeInternal(split1, end, hull1); 1308 #ifdef DEBUG_CONVEX_HULL 1309 printf("\n\nMerge\n"); 1310 result.print(); 1311 hull1.print(); 1312 #endif 1313 merge(result, hull1); 1314 #ifdef DEBUG_CONVEX_HULL 1315 printf("\n Result\n"); 1316 result.print(); 1317 #endif 1318 } 1319 1320 #ifdef DEBUG_CONVEX_HULL 1321 void btConvexHullInternal::IntermediateHull::print() 1322 { 1323 printf(" Hull\n"); 1324 for (Vertex* v = minXy; v; ) 1325 { 1326 printf(" "); 1327 v->print(); 1328 if (v == maxXy) 1329 { 1330 printf(" maxXy"); 1331 } 1332 if (v == minYx) 1333 { 1334 printf(" minYx"); 1335 } 1336 if (v == maxYx) 1337 { 1338 printf(" maxYx"); 1339 } 1340 if (v->next->prev != v) 1341 { 1342 printf(" Inconsistency"); 1343 } 1344 printf("\n"); 1345 v = v->next; 1346 if (v == minXy) 1347 { 1348 break; 1349 } 1350 } 1351 if (minXy) 1352 { 1353 minXy->copy = (minXy->copy == -1) ? -2 : -1; 1354 minXy->printGraph(); 1355 } 1356 } 1357 1358 void btConvexHullInternal::Vertex::printGraph() 1359 { 1360 print(); 1361 printf("\nEdges\n"); 1362 Edge* e = edges; 1363 if (e) 1364 { 1365 do 1366 { 1367 e->print(); 1368 printf("\n"); 1369 e = e->next; 1370 } while (e != edges); 1371 do 1372 { 1373 Vertex* v = e->target; 1374 if (v->copy != copy) 1375 { 1376 v->copy = copy; 1377 v->printGraph(); 1378 } 1379 e = e->next; 1380 } while (e != edges); 1381 } 1382 } 1383 #endif 1384 1385 btConvexHullInternal::Orientation btConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t) 1386 { 1387 btAssert(prev->reverse->target == next->reverse->target); 1388 if (prev->next == next) 1389 { 1390 if (prev->prev == next) 1391 { 1392 Point64 n = t.cross(s); 1393 Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target); 1394 btAssert(!m.isZero()); 1395 int64_t dot = n.dot(m); 1396 btAssert(dot != 0); 1397 return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE; 1398 } 1399 return COUNTER_CLOCKWISE; 1400 } 1401 else if (prev->prev == next) 1402 { 1403 return CLOCKWISE; 1404 } 1405 else 1406 { 1407 return NONE; 1408 } 1409 } 1410 1411 btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot) 1412 { 1413 Edge* minEdge = NULL; 1414 1415 #ifdef DEBUG_CONVEX_HULL 1416 printf("find max edge for %d\n", start->point.index); 1417 #endif 1418 Edge* e = start->edges; 1419 if (e) 1420 { 1421 do 1422 { 1423 if (e->copy > mergeStamp) 1424 { 1425 Point32 t = *e->target - *start; 1426 Rational64 cot(t.dot(sxrxs), t.dot(rxs)); 1427 #ifdef DEBUG_CONVEX_HULL 1428 printf(" Angle is %f (%d) for ", (float) btAtan(cot.toScalar()), (int) cot.isNaN()); 1429 e->print(); 1430 #endif 1431 if (cot.isNaN()) 1432 { 1433 btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0)); 1434 } 1435 else 1436 { 1437 int cmp; 1438 if (minEdge == NULL) 1439 { 1440 minCot = cot; 1441 minEdge = e; 1442 } 1443 else if ((cmp = cot.compare(minCot)) < 0) 1444 { 1445 minCot = cot; 1446 minEdge = e; 1447 } 1448 else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE))) 1449 { 1450 minEdge = e; 1451 } 1452 } 1453 #ifdef DEBUG_CONVEX_HULL 1454 printf("\n"); 1455 #endif 1456 } 1457 e = e->next; 1458 } while (e != start->edges); 1459 } 1460 return minEdge; 1461 } 1462 1463 void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1) 1464 { 1465 Edge* start0 = e0; 1466 Edge* start1 = e1; 1467 Point32 et0 = start0 ? start0->target->point : c0->point; 1468 Point32 et1 = start1 ? start1->target->point : c1->point; 1469 Point32 s = c1->point - c0->point; 1470 Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s); 1471 int64_t dist = c0->point.dot(normal); 1472 btAssert(!start1 || (start1->target->point.dot(normal) == dist)); 1473 Point64 perp = s.cross(normal); 1474 btAssert(!perp.isZero()); 1475 1476 #ifdef DEBUG_CONVEX_HULL 1477 printf(" Advancing %d %d (%p %p, %d %d)\n", c0->point.index, c1->point.index, start0, start1, start0 ? start0->target->point.index : -1, start1 ? start1->target->point.index : -1); 1478 #endif 1479 1480 int64_t maxDot0 = et0.dot(perp); 1481 if (e0) 1482 { 1483 while (e0->target != stop0) 1484 { 1485 Edge* e = e0->reverse->prev; 1486 if (e->target->point.dot(normal) < dist) 1487 { 1488 break; 1489 } 1490 btAssert(e->target->point.dot(normal) == dist); 1491 if (e->copy == mergeStamp) 1492 { 1493 break; 1494 } 1495 int64_t dot = e->target->point.dot(perp); 1496 if (dot <= maxDot0) 1497 { 1498 break; 1499 } 1500 maxDot0 = dot; 1501 e0 = e; 1502 et0 = e->target->point; 1503 } 1504 } 1505 1506 int64_t maxDot1 = et1.dot(perp); 1507 if (e1) 1508 { 1509 while (e1->target != stop1) 1510 { 1511 Edge* e = e1->reverse->next; 1512 if (e->target->point.dot(normal) < dist) 1513 { 1514 break; 1515 } 1516 btAssert(e->target->point.dot(normal) == dist); 1517 if (e->copy == mergeStamp) 1518 { 1519 break; 1520 } 1521 int64_t dot = e->target->point.dot(perp); 1522 if (dot <= maxDot1) 1523 { 1524 break; 1525 } 1526 maxDot1 = dot; 1527 e1 = e; 1528 et1 = e->target->point; 1529 } 1530 } 1531 1532 #ifdef DEBUG_CONVEX_HULL 1533 printf(" Starting at %d %d\n", et0.index, et1.index); 1534 #endif 1535 1536 int64_t dx = maxDot1 - maxDot0; 1537 if (dx > 0) 1538 { 1539 while (true) 1540 { 1541 int64_t dy = (et1 - et0).dot(s); 1542 1543 if (e0 && (e0->target != stop0)) 1544 { 1545 Edge* f0 = e0->next->reverse; 1546 if (f0->copy > mergeStamp) 1547 { 1548 int64_t dx0 = (f0->target->point - et0).dot(perp); 1549 int64_t dy0 = (f0->target->point - et0).dot(s); 1550 if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0))) 1551 { 1552 et0 = f0->target->point; 1553 dx = (et1 - et0).dot(perp); 1554 e0 = (e0 == start0) ? NULL : f0; 1555 continue; 1556 } 1557 } 1558 } 1559 1560 if (e1 && (e1->target != stop1)) 1561 { 1562 Edge* f1 = e1->reverse->next; 1563 if (f1->copy > mergeStamp) 1564 { 1565 Point32 d1 = f1->target->point - et1; 1566 if (d1.dot(normal) == 0) 1567 { 1568 int64_t dx1 = d1.dot(perp); 1569 int64_t dy1 = d1.dot(s); 1570 int64_t dxn = (f1->target->point - et0).dot(perp); 1571 if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0)))) 1572 { 1573 e1 = f1; 1574 et1 = e1->target->point; 1575 dx = dxn; 1576 continue; 1577 } 1578 } 1579 else 1580 { 1581 btAssert((e1 == start1) && (d1.dot(normal) < 0)); 1582 } 1583 } 1584 } 1585 1586 break; 1587 } 1588 } 1589 else if (dx < 0) 1590 { 1591 while (true) 1592 { 1593 int64_t dy = (et1 - et0).dot(s); 1594 1595 if (e1 && (e1->target != stop1)) 1596 { 1597 Edge* f1 = e1->prev->reverse; 1598 if (f1->copy > mergeStamp) 1599 { 1600 int64_t dx1 = (f1->target->point - et1).dot(perp); 1601 int64_t dy1 = (f1->target->point - et1).dot(s); 1602 if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0))) 1603 { 1604 et1 = f1->target->point; 1605 dx = (et1 - et0).dot(perp); 1606 e1 = (e1 == start1) ? NULL : f1; 1607 continue; 1608 } 1609 } 1610 } 1611 1612 if (e0 && (e0->target != stop0)) 1613 { 1614 Edge* f0 = e0->reverse->prev; 1615 if (f0->copy > mergeStamp) 1616 { 1617 Point32 d0 = f0->target->point - et0; 1618 if (d0.dot(normal) == 0) 1619 { 1620 int64_t dx0 = d0.dot(perp); 1621 int64_t dy0 = d0.dot(s); 1622 int64_t dxn = (et1 - f0->target->point).dot(perp); 1623 if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0)))) 1624 { 1625 e0 = f0; 1626 et0 = e0->target->point; 1627 dx = dxn; 1628 continue; 1629 } 1630 } 1631 else 1632 { 1633 btAssert((e0 == start0) && (d0.dot(normal) < 0)); 1634 } 1635 } 1636 } 1637 1638 break; 1639 } 1640 } 1641 #ifdef DEBUG_CONVEX_HULL 1642 printf(" Advanced edges to %d %d\n", et0.index, et1.index); 1643 #endif 1644 } 1645 1646 1647 void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) 1648 { 1649 if (!h1.maxXy) 1650 { 1651 return; 1652 } 1653 if (!h0.maxXy) 1654 { 1655 h0 = h1; 1656 return; 1657 } 1658 1659 mergeStamp--; 1660 1661 Vertex* c0 = NULL; 1662 Edge* toPrev0 = NULL; 1663 Edge* firstNew0 = NULL; 1664 Edge* pendingHead0 = NULL; 1665 Edge* pendingTail0 = NULL; 1666 Vertex* c1 = NULL; 1667 Edge* toPrev1 = NULL; 1668 Edge* firstNew1 = NULL; 1669 Edge* pendingHead1 = NULL; 1670 Edge* pendingTail1 = NULL; 1671 Point32 prevPoint; 1672 1673 if (mergeProjection(h0, h1, c0, c1)) 1674 { 1675 Point32 s = *c1 - *c0; 1676 Point64 normal = Point32(0, 0, -1).cross(s); 1677 Point64 t = s.cross(normal); 1678 btAssert(!t.isZero()); 1679 1680 Edge* e = c0->edges; 1681 Edge* start0 = NULL; 1682 if (e) 1683 { 1684 do 1685 { 1686 int64_t dot = (*e->target - *c0).dot(normal); 1687 btAssert(dot <= 0); 1688 if ((dot == 0) && ((*e->target - *c0).dot(t) > 0)) 1689 { 1690 if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE)) 1691 { 1692 start0 = e; 1693 } 1694 } 1695 e = e->next; 1696 } while (e != c0->edges); 1697 } 1698 1699 e = c1->edges; 1700 Edge* start1 = NULL; 1701 if (e) 1702 { 1703 do 1704 { 1705 int64_t dot = (*e->target - *c1).dot(normal); 1706 btAssert(dot <= 0); 1707 if ((dot == 0) && ((*e->target - *c1).dot(t) > 0)) 1708 { 1709 if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE)) 1710 { 1711 start1 = e; 1712 } 1713 } 1714 e = e->next; 1715 } while (e != c1->edges); 1716 } 1717 1718 if (start0 || start1) 1719 { 1720 findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL); 1721 if (start0) 1722 { 1723 c0 = start0->target; 1724 } 1725 if (start1) 1726 { 1727 c1 = start1->target; 1728 } 1729 } 1730 1731 prevPoint = c1->point; 1732 prevPoint.z++; 1733 } 1734 else 1735 { 1736 prevPoint = c1->point; 1737 prevPoint.x++; 1738 } 1739 1740 Vertex* first0 = c0; 1741 Vertex* first1 = c1; 1742 bool firstRun = true; 1743 1744 while (true) 1745 { 1746 Point32 s = *c1 - *c0; 1747 Point32 r = prevPoint - c0->point; 1748 Point64 rxs = r.cross(s); 1749 Point64 sxrxs = s.cross(rxs); 1750 1751 #ifdef DEBUG_CONVEX_HULL 1752 printf("\n Checking %d %d\n", c0->point.index, c1->point.index); 1753 #endif 1754 Rational64 minCot0(0, 0); 1755 Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0); 1756 Rational64 minCot1(0, 0); 1757 Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1); 1758 if (!min0 && !min1) 1759 { 1760 Edge* e = newEdgePair(c0, c1); 1761 e->link(e); 1762 c0->edges = e; 1763 1764 e = e->reverse; 1765 e->link(e); 1766 c1->edges = e; 1767 return; 1768 } 1769 else 1770 { 1771 int cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1); 1772 #ifdef DEBUG_CONVEX_HULL 1773 printf(" -> Result %d\n", cmp); 1774 #endif 1775 if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity())) 1776 { 1777 Edge* e = newEdgePair(c0, c1); 1778 if (pendingTail0) 1779 { 1780 pendingTail0->prev = e; 1781 } 1782 else 1783 { 1784 pendingHead0 = e; 1785 } 1786 e->next = pendingTail0; 1787 pendingTail0 = e; 1788 1789 e = e->reverse; 1790 if (pendingTail1) 1791 { 1792 pendingTail1->next = e; 1793 } 1794 else 1795 { 1796 pendingHead1 = e; 1797 } 1798 e->prev = pendingTail1; 1799 pendingTail1 = e; 1800 } 1801 1802 Edge* e0 = min0; 1803 Edge* e1 = min1; 1804 1805 #ifdef DEBUG_CONVEX_HULL 1806 printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1); 1807 #endif 1808 1809 if (cmp == 0) 1810 { 1811 findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL); 1812 } 1813 1814 if ((cmp >= 0) && e1) 1815 { 1816 if (toPrev1) 1817 { 1818 for (Edge* e = toPrev1->next, *n = NULL; e != min1; e = n) 1819 { 1820 n = e->next; 1821 removeEdgePair(e); 1822 } 1823 } 1824 1825 if (pendingTail1) 1826 { 1827 if (toPrev1) 1828 { 1829 toPrev1->link(pendingHead1); 1830 } 1831 else 1832 { 1833 min1->prev->link(pendingHead1); 1834 firstNew1 = pendingHead1; 1835 } 1836 pendingTail1->link(min1); 1837 pendingHead1 = NULL; 1838 pendingTail1 = NULL; 1839 } 1840 else if (!toPrev1) 1841 { 1842 firstNew1 = min1; 1843 } 1844 1845 prevPoint = c1->point; 1846 c1 = e1->target; 1847 toPrev1 = e1->reverse; 1848 } 1849 1850 if ((cmp <= 0) && e0) 1851 { 1852 if (toPrev0) 1853 { 1854 for (Edge* e = toPrev0->prev, *n = NULL; e != min0; e = n) 1855 { 1856 n = e->prev; 1857 removeEdgePair(e); 1858 } 1859 } 1860 1861 if (pendingTail0) 1862 { 1863 if (toPrev0) 1864 { 1865 pendingHead0->link(toPrev0); 1866 } 1867 else 1868 { 1869 pendingHead0->link(min0->next); 1870 firstNew0 = pendingHead0; 1871 } 1872 min0->link(pendingTail0); 1873 pendingHead0 = NULL; 1874 pendingTail0 = NULL; 1875 } 1876 else if (!toPrev0) 1877 { 1878 firstNew0 = min0; 1879 } 1880 1881 prevPoint = c0->point; 1882 c0 = e0->target; 1883 toPrev0 = e0->reverse; 1884 } 1885 } 1886 1887 if ((c0 == first0) && (c1 == first1)) 1888 { 1889 if (toPrev0 == NULL) 1890 { 1891 pendingHead0->link(pendingTail0); 1892 c0->edges = pendingTail0; 1893 } 1894 else 1895 { 1896 for (Edge* e = toPrev0->prev, *n = NULL; e != firstNew0; e = n) 1897 { 1898 n = e->prev; 1899 removeEdgePair(e); 1900 } 1901 if (pendingTail0) 1902 { 1903 pendingHead0->link(toPrev0); 1904 firstNew0->link(pendingTail0); 1905 } 1906 } 1907 1908 if (toPrev1 == NULL) 1909 { 1910 pendingTail1->link(pendingHead1); 1911 c1->edges = pendingTail1; 1912 } 1913 else 1914 { 1915 for (Edge* e = toPrev1->next, *n = NULL; e != firstNew1; e = n) 1916 { 1917 n = e->next; 1918 removeEdgePair(e); 1919 } 1920 if (pendingTail1) 1921 { 1922 toPrev1->link(pendingHead1); 1923 pendingTail1->link(firstNew1); 1924 } 1925 } 1926 1927 return; 1928 } 1929 1930 firstRun = false; 1931 } 1932 } 1933 1934 class pointCmp 1935 { 1936 public: 1937 1938 bool operator() ( const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q ) const 1939 { 1940 return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z)))); 1941 } 1942 }; 1943 1944 void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count) 1945 { 1946 btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30)); 1947 const char* ptr = (const char*) coords; 1948 if (doubleCoords) 1949 { 1950 for (int i = 0; i < count; i++) 1951 { 1952 const double* v = (const double*) ptr; 1953 btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]); 1954 ptr += stride; 1955 min.setMin(p); 1956 max.setMax(p); 1957 } 1958 } 1959 else 1960 { 1961 for (int i = 0; i < count; i++) 1962 { 1963 const float* v = (const float*) ptr; 1964 btVector3 p(v[0], v[1], v[2]); 1965 ptr += stride; 1966 min.setMin(p); 1967 max.setMax(p); 1968 } 1969 } 1970 1971 btVector3 s = max - min; 1972 maxAxis = s.maxAxis(); 1973 minAxis = s.minAxis(); 1974 if (minAxis == maxAxis) 1975 { 1976 minAxis = (maxAxis + 1) % 3; 1977 } 1978 medAxis = 3 - maxAxis - minAxis; 1979 1980 s /= btScalar(10216); 1981 if (((medAxis + 1) % 3) != maxAxis) 1982 { 1983 s *= -1; 1984 } 1985 scaling = s; 1986 1987 if (s[0] != 0) 1988 { 1989 s[0] = btScalar(1) / s[0]; 1990 } 1991 if (s[1] != 0) 1992 { 1993 s[1] = btScalar(1) / s[1]; 1994 } 1995 if (s[2] != 0) 1996 { 1997 s[2] = btScalar(1) / s[2]; 1998 } 1999 2000 center = (min + max) * btScalar(0.5); 2001 2002 btAlignedObjectArray<Point32> points; 2003 points.resize(count); 2004 ptr = (const char*) coords; 2005 if (doubleCoords) 2006 { 2007 for (int i = 0; i < count; i++) 2008 { 2009 const double* v = (const double*) ptr; 2010 btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]); 2011 ptr += stride; 2012 p = (p - center) * s; 2013 points[i].x = (int32_t) p[medAxis]; 2014 points[i].y = (int32_t) p[maxAxis]; 2015 points[i].z = (int32_t) p[minAxis]; 2016 points[i].index = i; 2017 } 2018 } 2019 else 2020 { 2021 for (int i = 0; i < count; i++) 2022 { 2023 const float* v = (const float*) ptr; 2024 btVector3 p(v[0], v[1], v[2]); 2025 ptr += stride; 2026 p = (p - center) * s; 2027 points[i].x = (int32_t) p[medAxis]; 2028 points[i].y = (int32_t) p[maxAxis]; 2029 points[i].z = (int32_t) p[minAxis]; 2030 points[i].index = i; 2031 } 2032 } 2033 points.quickSort(pointCmp()); 2034 2035 vertexPool.reset(); 2036 vertexPool.setArraySize(count); 2037 originalVertices.resize(count); 2038 for (int i = 0; i < count; i++) 2039 { 2040 Vertex* v = vertexPool.newObject(); 2041 v->edges = NULL; 2042 v->point = points[i]; 2043 v->copy = -1; 2044 originalVertices[i] = v; 2045 } 2046 2047 points.clear(); 2048 2049 edgePool.reset(); 2050 edgePool.setArraySize(6 * count); 2051 2052 usedEdgePairs = 0; 2053 maxUsedEdgePairs = 0; 2054 2055 mergeStamp = -3; 2056 2057 IntermediateHull hull; 2058 computeInternal(0, count, hull); 2059 vertexList = hull.minXy; 2060 #ifdef DEBUG_CONVEX_HULL 2061 printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count); 2062 #endif 2063 } 2064 2065 btVector3 btConvexHullInternal::toBtVector(const Point32& v) 2066 { 2067 btVector3 p; 2068 p[medAxis] = btScalar(v.x); 2069 p[maxAxis] = btScalar(v.y); 2070 p[minAxis] = btScalar(v.z); 2071 return p * scaling; 2072 } 2073 2074 btVector3 btConvexHullInternal::getBtNormal(Face* face) 2075 { 2076 return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized(); 2077 } 2078 2079 btVector3 btConvexHullInternal::getCoordinates(const Vertex* v) 2080 { 2081 btVector3 p; 2082 p[medAxis] = v->xvalue(); 2083 p[maxAxis] = v->yvalue(); 2084 p[minAxis] = v->zvalue(); 2085 return p * scaling + center; 2086 } 2087 2088 btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount) 2089 { 2090 if (!vertexList) 2091 { 2092 return 0; 2093 } 2094 int stamp = --mergeStamp; 2095 btAlignedObjectArray<Vertex*> stack; 2096 vertexList->copy = stamp; 2097 stack.push_back(vertexList); 2098 btAlignedObjectArray<Face*> faces; 2099 2100 Point32 ref = vertexList->point; 2101 Int128 hullCenterX(0, 0); 2102 Int128 hullCenterY(0, 0); 2103 Int128 hullCenterZ(0, 0); 2104 Int128 volume(0, 0); 2105 2106 while (stack.size() > 0) 2107 { 2108 Vertex* v = stack[stack.size() - 1]; 2109 stack.pop_back(); 2110 Edge* e = v->edges; 2111 if (e) 2112 { 2113 do 2114 { 2115 if (e->target->copy != stamp) 2116 { 2117 e->target->copy = stamp; 2118 stack.push_back(e->target); 2119 } 2120 if (e->copy != stamp) 2121 { 2122 Face* face = facePool.newObject(); 2123 face->init(e->target, e->reverse->prev->target, v); 2124 faces.push_back(face); 2125 Edge* f = e; 2126 2127 Vertex* a = NULL; 2128 Vertex* b = NULL; 2129 do 2130 { 2131 if (a && b) 2132 { 2133 int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref)); 2134 btAssert(vol >= 0); 2135 Point32 c = v->point + a->point + b->point + ref; 2136 hullCenterX += vol * c.x; 2137 hullCenterY += vol * c.y; 2138 hullCenterZ += vol * c.z; 2139 volume += vol; 2140 } 2141 2142 btAssert(f->copy != stamp); 2143 f->copy = stamp; 2144 f->face = face; 2145 2146 a = b; 2147 b = f->target; 2148 2149 f = f->reverse->prev; 2150 } while (f != e); 2151 } 2152 e = e->next; 2153 } while (e != v->edges); 2154 } 2155 } 2156 2157 if (volume.getSign() <= 0) 2158 { 2159 return 0; 2160 } 2161 2162 btVector3 hullCenter; 2163 hullCenter[medAxis] = hullCenterX.toScalar(); 2164 hullCenter[maxAxis] = hullCenterY.toScalar(); 2165 hullCenter[minAxis] = hullCenterZ.toScalar(); 2166 hullCenter /= 4 * volume.toScalar(); 2167 hullCenter *= scaling; 2168 2169 int faceCount = faces.size(); 2170 2171 if (clampAmount > 0) 2172 { 2173 btScalar minDist = SIMD_INFINITY; 2174 for (int i = 0; i < faceCount; i++) 2175 { 2176 btVector3 normal = getBtNormal(faces[i]); 2177 btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter); 2178 if (dist < minDist) 2179 { 2180 minDist = dist; 2181 } 2182 } 2183 2184 if (minDist <= 0) 2185 { 2186 return 0; 2187 } 2188 2189 amount = btMin(amount, minDist * clampAmount); 2190 } 2191 2192 unsigned int seed = 243703; 2193 for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223) 2194 { 2195 btSwap(faces[i], faces[seed % faceCount]); 2196 } 2197 2198 for (int i = 0; i < faceCount; i++) 2199 { 2200 if (!shiftFace(faces[i], amount, stack)) 2201 { 2202 return -amount; 2203 } 2204 } 2205 2206 return amount; 2207 } 2208 2209 bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack) 2210 { 2211 btVector3 origShift = getBtNormal(face) * -amount; 2212 if (scaling[0] != 0) 2213 { 2214 origShift[0] /= scaling[0]; 2215 } 2216 if (scaling[1] != 0) 2217 { 2218 origShift[1] /= scaling[1]; 2219 } 2220 if (scaling[2] != 0) 2221 { 2222 origShift[2] /= scaling[2]; 2223 } 2224 Point32 shift((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]); 2225 if (shift.isZero()) 2226 { 2227 return true; 2228 } 2229 Point64 normal = face->getNormal(); 2230 #ifdef DEBUG_CONVEX_HULL 2231 printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n", 2232 face->origin.x, face->origin.y, face->origin.z, face->dir0.x, face->dir0.y, face->dir0.z, face->dir1.x, face->dir1.y, face->dir1.z, shift.x, shift.y, shift.z); 2233 #endif 2234 int64_t origDot = face->origin.dot(normal); 2235 Point32 shiftedOrigin = face->origin + shift; 2236 int64_t shiftedDot = shiftedOrigin.dot(normal); 2237 btAssert(shiftedDot <= origDot); 2238 if (shiftedDot >= origDot) 2239 { 2240 return false; 2241 } 2242 2243 Edge* intersection = NULL; 2244 2245 Edge* startEdge = face->nearbyVertex->edges; 2246 #ifdef DEBUG_CONVEX_HULL 2247 printf("Start edge is "); 2248 startEdge->print(); 2249 printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot); 2250 #endif 2251 Rational128 optDot = face->nearbyVertex->dot(normal); 2252 int cmp = optDot.compare(shiftedDot); 2253 #ifdef SHOW_ITERATIONS 2254 int n = 0; 2255 #endif 2256 if (cmp >= 0) 2257 { 2258 Edge* e = startEdge; 2259 do 2260 { 2261 #ifdef SHOW_ITERATIONS 2262 n++; 2263 #endif 2264 Rational128 dot = e->target->dot(normal); 2265 btAssert(dot.compare(origDot) <= 0); 2266 #ifdef DEBUG_CONVEX_HULL 2267 printf("Moving downwards, edge is "); 2268 e->print(); 2269 printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot); 2270 #endif 2271 if (dot.compare(optDot) < 0) 2272 { 2273 int c = dot.compare(shiftedDot); 2274 optDot = dot; 2275 e = e->reverse; 2276 startEdge = e; 2277 if (c < 0) 2278 { 2279 intersection = e; 2280 break; 2281 } 2282 cmp = c; 2283 } 2284 e = e->prev; 2285 } while (e != startEdge); 2286 2287 if (!intersection) 2288 { 2289 return false; 2290 } 2291 } 2292 else 2293 { 2294 Edge* e = startEdge; 2295 do 2296 { 2297 #ifdef SHOW_ITERATIONS 2298 n++; 2299 #endif 2300 Rational128 dot = e->target->dot(normal); 2301 btAssert(dot.compare(origDot) <= 0); 2302 #ifdef DEBUG_CONVEX_HULL 2303 printf("Moving upwards, edge is "); 2304 e->print(); 2305 printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot); 2306 #endif 2307 if (dot.compare(optDot) > 0) 2308 { 2309 cmp = dot.compare(shiftedDot); 2310 if (cmp >= 0) 2311 { 2312 intersection = e; 2313 break; 2314 } 2315 optDot = dot; 2316 e = e->reverse; 2317 startEdge = e; 2318 } 2319 e = e->prev; 2320 } while (e != startEdge); 2321 2322 if (!intersection) 2323 { 2324 return true; 2325 } 2326 } 2327 2328 #ifdef SHOW_ITERATIONS 2329 printf("Needed %d iterations to find initial intersection\n", n); 2330 #endif 2331 2332 if (cmp == 0) 2333 { 2334 Edge* e = intersection->reverse->next; 2335 #ifdef SHOW_ITERATIONS 2336 n = 0; 2337 #endif 2338 while (e->target->dot(normal).compare(shiftedDot) <= 0) 2339 { 2340 #ifdef SHOW_ITERATIONS 2341 n++; 2342 #endif 2343 e = e->next; 2344 if (e == intersection->reverse) 2345 { 2346 return true; 2347 } 2348 #ifdef DEBUG_CONVEX_HULL 2349 printf("Checking for outwards edge, current edge is "); 2350 e->print(); 2351 printf("\n"); 2352 #endif 2353 } 2354 #ifdef SHOW_ITERATIONS 2355 printf("Needed %d iterations to check for complete containment\n", n); 2356 #endif 2357 } 2358 2359 Edge* firstIntersection = NULL; 2360 Edge* faceEdge = NULL; 2361 Edge* firstFaceEdge = NULL; 2362 2363 #ifdef SHOW_ITERATIONS 2364 int m = 0; 2365 #endif 2366 while (true) 2367 { 2368 #ifdef SHOW_ITERATIONS 2369 m++; 2370 #endif 2371 #ifdef DEBUG_CONVEX_HULL 2372 printf("Intersecting edge is "); 2373 intersection->print(); 2374 printf("\n"); 2375 #endif 2376 if (cmp == 0) 2377 { 2378 Edge* e = intersection->reverse->next; 2379 startEdge = e; 2380 #ifdef SHOW_ITERATIONS 2381 n = 0; 2382 #endif 2383 while (true) 2384 { 2385 #ifdef SHOW_ITERATIONS 2386 n++; 2387 #endif 2388 if (e->target->dot(normal).compare(shiftedDot) >= 0) 2389 { 2390 break; 2391 } 2392 intersection = e->reverse; 2393 e = e->next; 2394 if (e == startEdge) 2395 { 2396 return true; 2397 } 2398 } 2399 #ifdef SHOW_ITERATIONS 2400 printf("Needed %d iterations to advance intersection\n", n); 2401 #endif 2402 } 2403 2404 #ifdef DEBUG_CONVEX_HULL 2405 printf("Advanced intersecting edge to "); 2406 intersection->print(); 2407 printf(", cmp = %d\n", cmp); 2408 #endif 2409 2410 if (!firstIntersection) 2411 { 2412 firstIntersection = intersection; 2413 } 2414 else if (intersection == firstIntersection) 2415 { 2416 break; 2417 } 2418 2419 int prevCmp = cmp; 2420 Edge* prevIntersection = intersection; 2421 Edge* prevFaceEdge = faceEdge; 2422 2423 Edge* e = intersection->reverse; 2424 #ifdef SHOW_ITERATIONS 2425 n = 0; 2426 #endif 2427 while (true) 2428 { 2429 #ifdef SHOW_ITERATIONS 2430 n++; 2431 #endif 2432 e = e->reverse->prev; 2433 btAssert(e != intersection->reverse); 2434 cmp = e->target->dot(normal).compare(shiftedDot); 2435 #ifdef DEBUG_CONVEX_HULL 2436 printf("Testing edge "); 2437 e->print(); 2438 printf(" -> cmp = %d\n", cmp); 2439 #endif 2440 if (cmp >= 0) 2441 { 2442 intersection = e; 2443 break; 2444 } 2445 } 2446 #ifdef SHOW_ITERATIONS 2447 printf("Needed %d iterations to find other intersection of face\n", n); 2448 #endif 2449 2450 if (cmp > 0) 2451 { 2452 Vertex* removed = intersection->target; 2453 e = intersection->reverse; 2454 if (e->prev == e) 2455 { 2456 removed->edges = NULL; 2457 } 2458 else 2459 { 2460 removed->edges = e->prev; 2461 e->prev->link(e->next); 2462 e->link(e); 2463 } 2464 #ifdef DEBUG_CONVEX_HULL 2465 printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); 2466 #endif 2467 2468 Point64 n0 = intersection->face->getNormal(); 2469 Point64 n1 = intersection->reverse->face->getNormal(); 2470 int64_t m00 = face->dir0.dot(n0); 2471 int64_t m01 = face->dir1.dot(n0); 2472 int64_t m10 = face->dir0.dot(n1); 2473 int64_t m11 = face->dir1.dot(n1); 2474 int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0); 2475 int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1); 2476 Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10); 2477 btAssert(det.getSign() != 0); 2478 Vertex* v = vertexPool.newObject(); 2479 v->point.index = -1; 2480 v->copy = -1; 2481 v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01) 2482 + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x, 2483 Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01) 2484 + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y, 2485 Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01) 2486 + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z, 2487 det); 2488 v->point.x = (int32_t) v->point128.xvalue(); 2489 v->point.y = (int32_t) v->point128.yvalue(); 2490 v->point.z = (int32_t) v->point128.zvalue(); 2491 intersection->target = v; 2492 v->edges = e; 2493 2494 stack.push_back(v); 2495 stack.push_back(removed); 2496 stack.push_back(NULL); 2497 } 2498 2499 if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target)) 2500 { 2501 faceEdge = newEdgePair(prevIntersection->target, intersection->target); 2502 if (prevCmp == 0) 2503 { 2504 faceEdge->link(prevIntersection->reverse->next); 2505 } 2506 if ((prevCmp == 0) || prevFaceEdge) 2507 { 2508 prevIntersection->reverse->link(faceEdge); 2509 } 2510 if (cmp == 0) 2511 { 2512 intersection->reverse->prev->link(faceEdge->reverse); 2513 } 2514 faceEdge->reverse->link(intersection->reverse); 2515 } 2516 else 2517 { 2518 faceEdge = prevIntersection->reverse->next; 2519 } 2520 2521 if (prevFaceEdge) 2522 { 2523 if (prevCmp > 0) 2524 { 2525 faceEdge->link(prevFaceEdge->reverse); 2526 } 2527 else if (faceEdge != prevFaceEdge->reverse) 2528 { 2529 stack.push_back(prevFaceEdge->target); 2530 while (faceEdge->next != prevFaceEdge->reverse) 2531 { 2532 Vertex* removed = faceEdge->next->target; 2533 removeEdgePair(faceEdge->next); 2534 stack.push_back(removed); 2535 #ifdef DEBUG_CONVEX_HULL 2536 printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); 2537 #endif 2538 } 2539 stack.push_back(NULL); 2540 } 2541 } 2542 faceEdge->face = face; 2543 faceEdge->reverse->face = intersection->face; 2544 2545 if (!firstFaceEdge) 2546 { 2547 firstFaceEdge = faceEdge; 2548 } 2549 } 2550 #ifdef SHOW_ITERATIONS 2551 printf("Needed %d iterations to process all intersections\n", m); 2552 #endif 2553 2554 if (cmp > 0) 2555 { 2556 firstFaceEdge->reverse->target = faceEdge->target; 2557 firstIntersection->reverse->link(firstFaceEdge); 2558 firstFaceEdge->link(faceEdge->reverse); 2559 } 2560 else if (firstFaceEdge != faceEdge->reverse) 2561 { 2562 stack.push_back(faceEdge->target); 2563 while (firstFaceEdge->next != faceEdge->reverse) 2564 { 2565 Vertex* removed = firstFaceEdge->next->target; 2566 removeEdgePair(firstFaceEdge->next); 2567 stack.push_back(removed); 2568 #ifdef DEBUG_CONVEX_HULL 2569 printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); 2570 #endif 2571 } 2572 stack.push_back(NULL); 2573 } 2574 2575 btAssert(stack.size() > 0); 2576 vertexList = stack[0]; 2577 2578 #ifdef DEBUG_CONVEX_HULL 2579 printf("Removing part\n"); 2580 #endif 2581 #ifdef SHOW_ITERATIONS 2582 n = 0; 2583 #endif 2584 int pos = 0; 2585 while (pos < stack.size()) 2586 { 2587 int end = stack.size(); 2588 while (pos < end) 2589 { 2590 Vertex* kept = stack[pos++]; 2591 #ifdef DEBUG_CONVEX_HULL 2592 kept->print(); 2593 #endif 2594 bool deeper = false; 2595 Vertex* removed; 2596 while ((removed = stack[pos++]) != NULL) 2597 { 2598 #ifdef SHOW_ITERATIONS 2599 n++; 2600 #endif 2601 kept->receiveNearbyFaces(removed); 2602 while (removed->edges) 2603 { 2604 if (!deeper) 2605 { 2606 deeper = true; 2607 stack.push_back(kept); 2608 } 2609 stack.push_back(removed->edges->target); 2610 removeEdgePair(removed->edges); 2611 } 2612 } 2613 if (deeper) 2614 { 2615 stack.push_back(NULL); 2616 } 2617 } 2618 } 2619 #ifdef SHOW_ITERATIONS 2620 printf("Needed %d iterations to remove part\n", n); 2621 #endif 2622 2623 stack.resize(0); 2624 face->origin = shiftedOrigin; 2625 2626 return true; 2627 } 2628 2629 2630 static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices) 2631 { 2632 int index = vertex->copy; 2633 if (index < 0) 2634 { 2635 index = vertices.size(); 2636 vertex->copy = index; 2637 vertices.push_back(vertex); 2638 #ifdef DEBUG_CONVEX_HULL 2639 printf("Vertex %d gets index *%d\n", vertex->point.index, index); 2640 #endif 2641 } 2642 return index; 2643 } 2644 2645 btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp) 2646 { 2647 if (count <= 0) 2648 { 2649 vertices.clear(); 2650 edges.clear(); 2651 faces.clear(); 2652 return 0; 2653 } 2654 2655 btConvexHullInternal hull; 2656 hull.compute(coords, doubleCoords, stride, count); 2657 2658 btScalar shift = 0; 2659 if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0)) 2660 { 2661 vertices.clear(); 2662 edges.clear(); 2663 faces.clear(); 2664 return shift; 2665 } 2666 2667 vertices.resize(0); 2668 edges.resize(0); 2669 faces.resize(0); 2670 2671 btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices; 2672 getVertexCopy(hull.vertexList, oldVertices); 2673 int copied = 0; 2674 while (copied < oldVertices.size()) 2675 { 2676 btConvexHullInternal::Vertex* v = oldVertices[copied]; 2677 vertices.push_back(hull.getCoordinates(v)); 2678 btConvexHullInternal::Edge* firstEdge = v->edges; 2679 if (firstEdge) 2680 { 2681 int firstCopy = -1; 2682 int prevCopy = -1; 2683 btConvexHullInternal::Edge* e = firstEdge; 2684 do 2685 { 2686 if (e->copy < 0) 2687 { 2688 int s = edges.size(); 2689 edges.push_back(Edge()); 2690 edges.push_back(Edge()); 2691 Edge* c = &edges[s]; 2692 Edge* r = &edges[s + 1]; 2693 e->copy = s; 2694 e->reverse->copy = s + 1; 2695 c->reverse = 1; 2696 r->reverse = -1; 2697 c->targetVertex = getVertexCopy(e->target, oldVertices); 2698 r->targetVertex = copied; 2699 #ifdef DEBUG_CONVEX_HULL 2700 printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex()); 2701 #endif 2702 } 2703 if (prevCopy >= 0) 2704 { 2705 edges[e->copy].next = prevCopy - e->copy; 2706 } 2707 else 2708 { 2709 firstCopy = e->copy; 2710 } 2711 prevCopy = e->copy; 2712 e = e->next; 2713 } while (e != firstEdge); 2714 edges[firstCopy].next = prevCopy - firstCopy; 2715 } 2716 copied++; 2717 } 2718 2719 for (int i = 0; i < copied; i++) 2720 { 2721 btConvexHullInternal::Vertex* v = oldVertices[i]; 2722 btConvexHullInternal::Edge* firstEdge = v->edges; 2723 if (firstEdge) 2724 { 2725 btConvexHullInternal::Edge* e = firstEdge; 2726 do 2727 { 2728 if (e->copy >= 0) 2729 { 2730 #ifdef DEBUG_CONVEX_HULL 2731 printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex()); 2732 #endif 2733 faces.push_back(e->copy); 2734 btConvexHullInternal::Edge* f = e; 2735 do 2736 { 2737 #ifdef DEBUG_CONVEX_HULL 2738 printf(" Face *%d\n", edges[f->copy].getTargetVertex()); 2739 #endif 2740 f->copy = -1; 2741 f = f->reverse->prev; 2742 } while (f != e); 2743 } 2744 e = e->next; 2745 } while (e != firstEdge); 2746 } 2747 } 2748 2749 return shift; 2750 } 2751 2752 2753 2754 2755 2756
