Bullet Collision Detection & Physics Library
btConvexHullComputer.cpp
Go to the documentation of this file.
1/*
2Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
3
4This software is provided 'as-is', without any express or implied warranty.
5In no event will the authors be held liable for any damages arising from the use of this software.
6Permission is granted to anyone to use this software for any purpose,
7including commercial applications, and to alter it and redistribute it freely,
8subject to the following restrictions:
9
101. 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.
112. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
123. This notice may not be removed or altered from any source distribution.
13*/
14
15#include <string.h>
16
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
53{
54 public:
55
56 class Point64
57 {
58 public:
62
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:
84 int index;
85
87 {
88 }
89
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:
145
147 {
148 }
149
151 {
152 }
153
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
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
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
223 {
224 if (++low == 0)
225 {
226 ++high;
227 }
228 return *this;
229 }
230
231 Int128 operator*(int64_t b) const;
232
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
273 {
274 private:
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
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
325 {
327 }
328 };
329
330
332 {
333 private:
336 int sign;
338
339 public:
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
362 {
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
390 {
392 }
393 };
394
396 {
397 public:
402
404 {
405 }
406
408 {
409 }
410
412 {
413 return x.toScalar() / denominator.toScalar();
414 }
415
417 {
418 return y.toScalar() / denominator.toScalar();
419 }
420
422 {
423 return z.toScalar() / denominator.toScalar();
424 }
425 };
426
427
428 class Edge;
429 class Face;
430
431 class Vertex
432 {
433 public:
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))
465 }
466
468 {
469 return (point.index >= 0) ? btScalar(point.x) : point128.xvalue();
470 }
471
473 {
474 return (point.index >= 0) ? btScalar(point.y) : point128.yvalue();
475 }
476
478 {
479 return (point.index >= 0) ? btScalar(point.z) : point128.zvalue();
480 }
481
483 {
484 if (lastNearbyFace)
485 {
487 }
488 else
489 {
491 }
492 if (src->lastNearbyFace)
493 {
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:
515 int copy;
516
518 {
519 next = NULL;
520 prev = NULL;
521 reverse = NULL;
522 target = NULL;
523 face = NULL;
524 }
525
526 void link(Edge* n)
527 {
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,
538 }
539#endif
540 };
541
542 class Face
543 {
544 public:
551
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 {
565 }
566 else
567 {
568 a->firstNearbyFace = this;
569 }
570 a->lastNearbyFace = this;
571 }
572
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
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
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
649 {
650 public:
655
656 IntermediateHull(): minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL)
657 {
658 }
659
660 void print();
661 };
662
664
665 template <typename T> class PoolArray
666 {
667 private:
669 int size;
670
671 public:
673
675 {
676 array = (T*) btAlignedAlloc(sizeof(T) * size, 16);
677 }
678
680 {
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:
702
703 public:
704 Pool(): arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256)
705 {
706 }
707
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 {
722 freeObjects = NULL;
723 }
724
726 {
727 this->arraySize = arraySize;
728 }
729
731 {
732 T* o = freeObjects;
733 if (!o)
734 {
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
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
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);
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:
829
830 void compute(const void* coords, bool doubleCoords, int stride, int count);
831
833
834 btScalar shrink(btScalar amount, btScalar clampAmount);
835};
836
837
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
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
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
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
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
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
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++;
1013 {
1015 }
1016 return e;
1017}
1018
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
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 Vertex* v = originalVertices[start];
1282 v->edges = NULL;
1283 v->next = v;
1284 v->prev = v;
1285
1286 result.minXy = v;
1287 result.maxXy = v;
1288 result.minYx = v;
1289 result.maxYx = v;
1290 }
1291
1292 return;
1293 }
1294
1295 case 1:
1296 {
1297 Vertex* v = originalVertices[start];
1298 v->edges = NULL;
1299 v->next = v;
1300 v->prev = v;
1301
1302 result.minXy = v;
1303 result.maxXy = v;
1304 result.minYx = v;
1305 result.maxYx = v;
1306
1307 return;
1308 }
1309 }
1310
1311 int split0 = start + n / 2;
1312 Point32 p = originalVertices[split0-1]->point;
1313 int split1 = split0;
1314 while ((split1 < end) && (originalVertices[split1]->point == p))
1315 {
1316 split1++;
1317 }
1318 computeInternal(start, split0, result);
1319 IntermediateHull hull1;
1320 computeInternal(split1, end, hull1);
1321#ifdef DEBUG_CONVEX_HULL
1322 printf("\n\nMerge\n");
1323 result.print();
1324 hull1.print();
1325#endif
1326 merge(result, hull1);
1327#ifdef DEBUG_CONVEX_HULL
1328 printf("\n Result\n");
1329 result.print();
1330#endif
1331}
1332
1333#ifdef DEBUG_CONVEX_HULL
1335{
1336 printf(" Hull\n");
1337 for (Vertex* v = minXy; v; )
1338 {
1339 printf(" ");
1340 v->print();
1341 if (v == maxXy)
1342 {
1343 printf(" maxXy");
1344 }
1345 if (v == minYx)
1346 {
1347 printf(" minYx");
1348 }
1349 if (v == maxYx)
1350 {
1351 printf(" maxYx");
1352 }
1353 if (v->next->prev != v)
1354 {
1355 printf(" Inconsistency");
1356 }
1357 printf("\n");
1358 v = v->next;
1359 if (v == minXy)
1360 {
1361 break;
1362 }
1363 }
1364 if (minXy)
1365 {
1366 minXy->copy = (minXy->copy == -1) ? -2 : -1;
1367 minXy->printGraph();
1368 }
1369}
1370
1371void btConvexHullInternal::Vertex::printGraph()
1372{
1373 print();
1374 printf("\nEdges\n");
1375 Edge* e = edges;
1376 if (e)
1377 {
1378 do
1379 {
1380 e->print();
1381 printf("\n");
1382 e = e->next;
1383 } while (e != edges);
1384 do
1385 {
1386 Vertex* v = e->target;
1387 if (v->copy != copy)
1388 {
1389 v->copy = copy;
1390 v->printGraph();
1391 }
1392 e = e->next;
1393 } while (e != edges);
1394 }
1395}
1396#endif
1397
1399{
1400 btAssert(prev->reverse->target == next->reverse->target);
1401 if (prev->next == next)
1402 {
1403 if (prev->prev == next)
1404 {
1405 Point64 n = t.cross(s);
1406 Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target);
1407 btAssert(!m.isZero());
1408 int64_t dot = n.dot(m);
1409 btAssert(dot != 0);
1410 return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE;
1411 }
1412 return COUNTER_CLOCKWISE;
1413 }
1414 else if (prev->prev == next)
1415 {
1416 return CLOCKWISE;
1417 }
1418 else
1419 {
1420 return NONE;
1421 }
1422}
1423
1424btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot)
1425{
1426 Edge* minEdge = NULL;
1427
1428#ifdef DEBUG_CONVEX_HULL
1429 printf("find max edge for %d\n", start->point.index);
1430#endif
1431 Edge* e = start->edges;
1432 if (e)
1433 {
1434 do
1435 {
1436 if (e->copy > mergeStamp)
1437 {
1438 Point32 t = *e->target - *start;
1439 Rational64 cot(t.dot(sxrxs), t.dot(rxs));
1440#ifdef DEBUG_CONVEX_HULL
1441 printf(" Angle is %f (%d) for ", (float) btAtan(cot.toScalar()), (int) cot.isNaN());
1442 e->print();
1443#endif
1444 if (cot.isNaN())
1445 {
1446 btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0));
1447 }
1448 else
1449 {
1450 int cmp;
1451 if (minEdge == NULL)
1452 {
1453 minCot = cot;
1454 minEdge = e;
1455 }
1456 else if ((cmp = cot.compare(minCot)) < 0)
1457 {
1458 minCot = cot;
1459 minEdge = e;
1460 }
1461 else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE)))
1462 {
1463 minEdge = e;
1464 }
1465 }
1466#ifdef DEBUG_CONVEX_HULL
1467 printf("\n");
1468#endif
1469 }
1470 e = e->next;
1471 } while (e != start->edges);
1472 }
1473 return minEdge;
1474}
1475
1477{
1478 Edge* start0 = e0;
1479 Edge* start1 = e1;
1480 Point32 et0 = start0 ? start0->target->point : c0->point;
1481 Point32 et1 = start1 ? start1->target->point : c1->point;
1482 Point32 s = c1->point - c0->point;
1483 Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s);
1484 int64_t dist = c0->point.dot(normal);
1485 btAssert(!start1 || (start1->target->point.dot(normal) == dist));
1486 Point64 perp = s.cross(normal);
1487 btAssert(!perp.isZero());
1488
1489#ifdef DEBUG_CONVEX_HULL
1490 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);
1491#endif
1492
1493 int64_t maxDot0 = et0.dot(perp);
1494 if (e0)
1495 {
1496 while (e0->target != stop0)
1497 {
1498 Edge* e = e0->reverse->prev;
1499 if (e->target->point.dot(normal) < dist)
1500 {
1501 break;
1502 }
1503 btAssert(e->target->point.dot(normal) == dist);
1504 if (e->copy == mergeStamp)
1505 {
1506 break;
1507 }
1508 int64_t dot = e->target->point.dot(perp);
1509 if (dot <= maxDot0)
1510 {
1511 break;
1512 }
1513 maxDot0 = dot;
1514 e0 = e;
1515 et0 = e->target->point;
1516 }
1517 }
1518
1519 int64_t maxDot1 = et1.dot(perp);
1520 if (e1)
1521 {
1522 while (e1->target != stop1)
1523 {
1524 Edge* e = e1->reverse->next;
1525 if (e->target->point.dot(normal) < dist)
1526 {
1527 break;
1528 }
1529 btAssert(e->target->point.dot(normal) == dist);
1530 if (e->copy == mergeStamp)
1531 {
1532 break;
1533 }
1534 int64_t dot = e->target->point.dot(perp);
1535 if (dot <= maxDot1)
1536 {
1537 break;
1538 }
1539 maxDot1 = dot;
1540 e1 = e;
1541 et1 = e->target->point;
1542 }
1543 }
1544
1545#ifdef DEBUG_CONVEX_HULL
1546 printf(" Starting at %d %d\n", et0.index, et1.index);
1547#endif
1548
1549 int64_t dx = maxDot1 - maxDot0;
1550 if (dx > 0)
1551 {
1552 while (true)
1553 {
1554 int64_t dy = (et1 - et0).dot(s);
1555
1556 if (e0 && (e0->target != stop0))
1557 {
1558 Edge* f0 = e0->next->reverse;
1559 if (f0->copy > mergeStamp)
1560 {
1561 int64_t dx0 = (f0->target->point - et0).dot(perp);
1562 int64_t dy0 = (f0->target->point - et0).dot(s);
1563 if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0)))
1564 {
1565 et0 = f0->target->point;
1566 dx = (et1 - et0).dot(perp);
1567 e0 = (e0 == start0) ? NULL : f0;
1568 continue;
1569 }
1570 }
1571 }
1572
1573 if (e1 && (e1->target != stop1))
1574 {
1575 Edge* f1 = e1->reverse->next;
1576 if (f1->copy > mergeStamp)
1577 {
1578 Point32 d1 = f1->target->point - et1;
1579 if (d1.dot(normal) == 0)
1580 {
1581 int64_t dx1 = d1.dot(perp);
1582 int64_t dy1 = d1.dot(s);
1583 int64_t dxn = (f1->target->point - et0).dot(perp);
1584 if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0))))
1585 {
1586 e1 = f1;
1587 et1 = e1->target->point;
1588 dx = dxn;
1589 continue;
1590 }
1591 }
1592 else
1593 {
1594 btAssert((e1 == start1) && (d1.dot(normal) < 0));
1595 }
1596 }
1597 }
1598
1599 break;
1600 }
1601 }
1602 else if (dx < 0)
1603 {
1604 while (true)
1605 {
1606 int64_t dy = (et1 - et0).dot(s);
1607
1608 if (e1 && (e1->target != stop1))
1609 {
1610 Edge* f1 = e1->prev->reverse;
1611 if (f1->copy > mergeStamp)
1612 {
1613 int64_t dx1 = (f1->target->point - et1).dot(perp);
1614 int64_t dy1 = (f1->target->point - et1).dot(s);
1615 if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0)))
1616 {
1617 et1 = f1->target->point;
1618 dx = (et1 - et0).dot(perp);
1619 e1 = (e1 == start1) ? NULL : f1;
1620 continue;
1621 }
1622 }
1623 }
1624
1625 if (e0 && (e0->target != stop0))
1626 {
1627 Edge* f0 = e0->reverse->prev;
1628 if (f0->copy > mergeStamp)
1629 {
1630 Point32 d0 = f0->target->point - et0;
1631 if (d0.dot(normal) == 0)
1632 {
1633 int64_t dx0 = d0.dot(perp);
1634 int64_t dy0 = d0.dot(s);
1635 int64_t dxn = (et1 - f0->target->point).dot(perp);
1636 if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0))))
1637 {
1638 e0 = f0;
1639 et0 = e0->target->point;
1640 dx = dxn;
1641 continue;
1642 }
1643 }
1644 else
1645 {
1646 btAssert((e0 == start0) && (d0.dot(normal) < 0));
1647 }
1648 }
1649 }
1650
1651 break;
1652 }
1653 }
1654#ifdef DEBUG_CONVEX_HULL
1655 printf(" Advanced edges to %d %d\n", et0.index, et1.index);
1656#endif
1657}
1658
1659
1661{
1662 if (!h1.maxXy)
1663 {
1664 return;
1665 }
1666 if (!h0.maxXy)
1667 {
1668 h0 = h1;
1669 return;
1670 }
1671
1672 mergeStamp--;
1673
1674 Vertex* c0 = NULL;
1675 Edge* toPrev0 = NULL;
1676 Edge* firstNew0 = NULL;
1677 Edge* pendingHead0 = NULL;
1678 Edge* pendingTail0 = NULL;
1679 Vertex* c1 = NULL;
1680 Edge* toPrev1 = NULL;
1681 Edge* firstNew1 = NULL;
1682 Edge* pendingHead1 = NULL;
1683 Edge* pendingTail1 = NULL;
1684 Point32 prevPoint;
1685
1686 if (mergeProjection(h0, h1, c0, c1))
1687 {
1688 Point32 s = *c1 - *c0;
1689 Point64 normal = Point32(0, 0, -1).cross(s);
1690 Point64 t = s.cross(normal);
1691 btAssert(!t.isZero());
1692
1693 Edge* e = c0->edges;
1694 Edge* start0 = NULL;
1695 if (e)
1696 {
1697 do
1698 {
1699 int64_t dot = (*e->target - *c0).dot(normal);
1700 btAssert(dot <= 0);
1701 if ((dot == 0) && ((*e->target - *c0).dot(t) > 0))
1702 {
1703 if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE))
1704 {
1705 start0 = e;
1706 }
1707 }
1708 e = e->next;
1709 } while (e != c0->edges);
1710 }
1711
1712 e = c1->edges;
1713 Edge* start1 = NULL;
1714 if (e)
1715 {
1716 do
1717 {
1718 int64_t dot = (*e->target - *c1).dot(normal);
1719 btAssert(dot <= 0);
1720 if ((dot == 0) && ((*e->target - *c1).dot(t) > 0))
1721 {
1722 if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE))
1723 {
1724 start1 = e;
1725 }
1726 }
1727 e = e->next;
1728 } while (e != c1->edges);
1729 }
1730
1731 if (start0 || start1)
1732 {
1733 findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL);
1734 if (start0)
1735 {
1736 c0 = start0->target;
1737 }
1738 if (start1)
1739 {
1740 c1 = start1->target;
1741 }
1742 }
1743
1744 prevPoint = c1->point;
1745 prevPoint.z++;
1746 }
1747 else
1748 {
1749 prevPoint = c1->point;
1750 prevPoint.x++;
1751 }
1752
1753 Vertex* first0 = c0;
1754 Vertex* first1 = c1;
1755 bool firstRun = true;
1756
1757 while (true)
1758 {
1759 Point32 s = *c1 - *c0;
1760 Point32 r = prevPoint - c0->point;
1761 Point64 rxs = r.cross(s);
1762 Point64 sxrxs = s.cross(rxs);
1763
1764#ifdef DEBUG_CONVEX_HULL
1765 printf("\n Checking %d %d\n", c0->point.index, c1->point.index);
1766#endif
1767 Rational64 minCot0(0, 0);
1768 Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0);
1769 Rational64 minCot1(0, 0);
1770 Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1);
1771 if (!min0 && !min1)
1772 {
1773 Edge* e = newEdgePair(c0, c1);
1774 e->link(e);
1775 c0->edges = e;
1776
1777 e = e->reverse;
1778 e->link(e);
1779 c1->edges = e;
1780 return;
1781 }
1782 else
1783 {
1784 int cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1);
1785#ifdef DEBUG_CONVEX_HULL
1786 printf(" -> Result %d\n", cmp);
1787#endif
1788 if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity()))
1789 {
1790 Edge* e = newEdgePair(c0, c1);
1791 if (pendingTail0)
1792 {
1793 pendingTail0->prev = e;
1794 }
1795 else
1796 {
1797 pendingHead0 = e;
1798 }
1799 e->next = pendingTail0;
1800 pendingTail0 = e;
1801
1802 e = e->reverse;
1803 if (pendingTail1)
1804 {
1805 pendingTail1->next = e;
1806 }
1807 else
1808 {
1809 pendingHead1 = e;
1810 }
1811 e->prev = pendingTail1;
1812 pendingTail1 = e;
1813 }
1814
1815 Edge* e0 = min0;
1816 Edge* e1 = min1;
1817
1818#ifdef DEBUG_CONVEX_HULL
1819 printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1);
1820#endif
1821
1822 if (cmp == 0)
1823 {
1824 findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL);
1825 }
1826
1827 if ((cmp >= 0) && e1)
1828 {
1829 if (toPrev1)
1830 {
1831 for (Edge* e = toPrev1->next, *n = NULL; e != min1; e = n)
1832 {
1833 n = e->next;
1834 removeEdgePair(e);
1835 }
1836 }
1837
1838 if (pendingTail1)
1839 {
1840 if (toPrev1)
1841 {
1842 toPrev1->link(pendingHead1);
1843 }
1844 else
1845 {
1846 min1->prev->link(pendingHead1);
1847 firstNew1 = pendingHead1;
1848 }
1849 pendingTail1->link(min1);
1850 pendingHead1 = NULL;
1851 pendingTail1 = NULL;
1852 }
1853 else if (!toPrev1)
1854 {
1855 firstNew1 = min1;
1856 }
1857
1858 prevPoint = c1->point;
1859 c1 = e1->target;
1860 toPrev1 = e1->reverse;
1861 }
1862
1863 if ((cmp <= 0) && e0)
1864 {
1865 if (toPrev0)
1866 {
1867 for (Edge* e = toPrev0->prev, *n = NULL; e != min0; e = n)
1868 {
1869 n = e->prev;
1870 removeEdgePair(e);
1871 }
1872 }
1873
1874 if (pendingTail0)
1875 {
1876 if (toPrev0)
1877 {
1878 pendingHead0->link(toPrev0);
1879 }
1880 else
1881 {
1882 pendingHead0->link(min0->next);
1883 firstNew0 = pendingHead0;
1884 }
1885 min0->link(pendingTail0);
1886 pendingHead0 = NULL;
1887 pendingTail0 = NULL;
1888 }
1889 else if (!toPrev0)
1890 {
1891 firstNew0 = min0;
1892 }
1893
1894 prevPoint = c0->point;
1895 c0 = e0->target;
1896 toPrev0 = e0->reverse;
1897 }
1898 }
1899
1900 if ((c0 == first0) && (c1 == first1))
1901 {
1902 if (toPrev0 == NULL)
1903 {
1904 pendingHead0->link(pendingTail0);
1905 c0->edges = pendingTail0;
1906 }
1907 else
1908 {
1909 for (Edge* e = toPrev0->prev, *n = NULL; e != firstNew0; e = n)
1910 {
1911 n = e->prev;
1912 removeEdgePair(e);
1913 }
1914 if (pendingTail0)
1915 {
1916 pendingHead0->link(toPrev0);
1917 firstNew0->link(pendingTail0);
1918 }
1919 }
1920
1921 if (toPrev1 == NULL)
1922 {
1923 pendingTail1->link(pendingHead1);
1924 c1->edges = pendingTail1;
1925 }
1926 else
1927 {
1928 for (Edge* e = toPrev1->next, *n = NULL; e != firstNew1; e = n)
1929 {
1930 n = e->next;
1931 removeEdgePair(e);
1932 }
1933 if (pendingTail1)
1934 {
1935 toPrev1->link(pendingHead1);
1936 pendingTail1->link(firstNew1);
1937 }
1938 }
1939
1940 return;
1941 }
1942
1943 firstRun = false;
1944 }
1945}
1946
1948{
1949 public:
1950
1951 bool operator() ( const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q ) const
1952 {
1953 return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
1954 }
1955};
1956
1957void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
1958{
1959 btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30));
1960 const char* ptr = (const char*) coords;
1961 if (doubleCoords)
1962 {
1963 for (int i = 0; i < count; i++)
1964 {
1965 const double* v = (const double*) ptr;
1966 btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
1967 ptr += stride;
1968 min.setMin(p);
1969 max.setMax(p);
1970 }
1971 }
1972 else
1973 {
1974 for (int i = 0; i < count; i++)
1975 {
1976 const float* v = (const float*) ptr;
1977 btVector3 p(v[0], v[1], v[2]);
1978 ptr += stride;
1979 min.setMin(p);
1980 max.setMax(p);
1981 }
1982 }
1983
1984 btVector3 s = max - min;
1985 maxAxis = s.maxAxis();
1986 minAxis = s.minAxis();
1987 if (minAxis == maxAxis)
1988 {
1989 minAxis = (maxAxis + 1) % 3;
1990 }
1991 medAxis = 3 - maxAxis - minAxis;
1992
1993 s /= btScalar(10216);
1994 if (((medAxis + 1) % 3) != maxAxis)
1995 {
1996 s *= -1;
1997 }
1998 scaling = s;
1999
2000 if (s[0] != 0)
2001 {
2002 s[0] = btScalar(1) / s[0];
2003 }
2004 if (s[1] != 0)
2005 {
2006 s[1] = btScalar(1) / s[1];
2007 }
2008 if (s[2] != 0)
2009 {
2010 s[2] = btScalar(1) / s[2];
2011 }
2012
2013 center = (min + max) * btScalar(0.5);
2014
2016 points.resize(count);
2017 ptr = (const char*) coords;
2018 if (doubleCoords)
2019 {
2020 for (int i = 0; i < count; i++)
2021 {
2022 const double* v = (const double*) ptr;
2023 btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
2024 ptr += stride;
2025 p = (p - center) * s;
2026 points[i].x = (int32_t) p[medAxis];
2027 points[i].y = (int32_t) p[maxAxis];
2028 points[i].z = (int32_t) p[minAxis];
2029 points[i].index = i;
2030 }
2031 }
2032 else
2033 {
2034 for (int i = 0; i < count; i++)
2035 {
2036 const float* v = (const float*) ptr;
2037 btVector3 p(v[0], v[1], v[2]);
2038 ptr += stride;
2039 p = (p - center) * s;
2040 points[i].x = (int32_t) p[medAxis];
2041 points[i].y = (int32_t) p[maxAxis];
2042 points[i].z = (int32_t) p[minAxis];
2043 points[i].index = i;
2044 }
2045 }
2046 points.quickSort(pointCmp());
2047
2048 vertexPool.reset();
2049 vertexPool.setArraySize(count);
2050 originalVertices.resize(count);
2051 for (int i = 0; i < count; i++)
2052 {
2053 Vertex* v = vertexPool.newObject();
2054 v->edges = NULL;
2055 v->point = points[i];
2056 v->copy = -1;
2057 originalVertices[i] = v;
2058 }
2059
2060 points.clear();
2061
2062 edgePool.reset();
2063 edgePool.setArraySize(6 * count);
2064
2065 usedEdgePairs = 0;
2066 maxUsedEdgePairs = 0;
2067
2068 mergeStamp = -3;
2069
2070 IntermediateHull hull;
2071 computeInternal(0, count, hull);
2072 vertexList = hull.minXy;
2073#ifdef DEBUG_CONVEX_HULL
2074 printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count);
2075#endif
2076}
2077
2079{
2080 btVector3 p;
2081 p[medAxis] = btScalar(v.x);
2082 p[maxAxis] = btScalar(v.y);
2083 p[minAxis] = btScalar(v.z);
2084 return p * scaling;
2085}
2086
2088{
2089 return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized();
2090}
2091
2093{
2094 btVector3 p;
2095 p[medAxis] = v->xvalue();
2096 p[maxAxis] = v->yvalue();
2097 p[minAxis] = v->zvalue();
2098 return p * scaling + center;
2099}
2100
2102{
2103 if (!vertexList)
2104 {
2105 return 0;
2106 }
2107 int stamp = --mergeStamp;
2109 vertexList->copy = stamp;
2110 stack.push_back(vertexList);
2112
2113 Point32 ref = vertexList->point;
2114 Int128 hullCenterX(0, 0);
2115 Int128 hullCenterY(0, 0);
2116 Int128 hullCenterZ(0, 0);
2117 Int128 volume(0, 0);
2118
2119 while (stack.size() > 0)
2120 {
2121 Vertex* v = stack[stack.size() - 1];
2122 stack.pop_back();
2123 Edge* e = v->edges;
2124 if (e)
2125 {
2126 do
2127 {
2128 if (e->target->copy != stamp)
2129 {
2130 e->target->copy = stamp;
2131 stack.push_back(e->target);
2132 }
2133 if (e->copy != stamp)
2134 {
2135 Face* face = facePool.newObject();
2136 face->init(e->target, e->reverse->prev->target, v);
2137 faces.push_back(face);
2138 Edge* f = e;
2139
2140 Vertex* a = NULL;
2141 Vertex* b = NULL;
2142 do
2143 {
2144 if (a && b)
2145 {
2146 int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
2147 btAssert(vol >= 0);
2148 Point32 c = v->point + a->point + b->point + ref;
2149 hullCenterX += vol * c.x;
2150 hullCenterY += vol * c.y;
2151 hullCenterZ += vol * c.z;
2152 volume += vol;
2153 }
2154
2155 btAssert(f->copy != stamp);
2156 f->copy = stamp;
2157 f->face = face;
2158
2159 a = b;
2160 b = f->target;
2161
2162 f = f->reverse->prev;
2163 } while (f != e);
2164 }
2165 e = e->next;
2166 } while (e != v->edges);
2167 }
2168 }
2169
2170 if (volume.getSign() <= 0)
2171 {
2172 return 0;
2173 }
2174
2175 btVector3 hullCenter;
2176 hullCenter[medAxis] = hullCenterX.toScalar();
2177 hullCenter[maxAxis] = hullCenterY.toScalar();
2178 hullCenter[minAxis] = hullCenterZ.toScalar();
2179 hullCenter /= 4 * volume.toScalar();
2180 hullCenter *= scaling;
2181
2182 int faceCount = faces.size();
2183
2184 if (clampAmount > 0)
2185 {
2186 btScalar minDist = SIMD_INFINITY;
2187 for (int i = 0; i < faceCount; i++)
2188 {
2189 btVector3 normal = getBtNormal(faces[i]);
2190 btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
2191 if (dist < minDist)
2192 {
2193 minDist = dist;
2194 }
2195 }
2196
2197 if (minDist <= 0)
2198 {
2199 return 0;
2200 }
2201
2202 amount = btMin(amount, minDist * clampAmount);
2203 }
2204
2205 unsigned int seed = 243703;
2206 for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
2207 {
2208 btSwap(faces[i], faces[seed % faceCount]);
2209 }
2210
2211 for (int i = 0; i < faceCount; i++)
2212 {
2213 if (!shiftFace(faces[i], amount, stack))
2214 {
2215 return -amount;
2216 }
2217 }
2218
2219 return amount;
2220}
2221
2223{
2224 btVector3 origShift = getBtNormal(face) * -amount;
2225 if (scaling[0] != 0)
2226 {
2227 origShift[0] /= scaling[0];
2228 }
2229 if (scaling[1] != 0)
2230 {
2231 origShift[1] /= scaling[1];
2232 }
2233 if (scaling[2] != 0)
2234 {
2235 origShift[2] /= scaling[2];
2236 }
2237 Point32 shift((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]);
2238 if (shift.isZero())
2239 {
2240 return true;
2241 }
2242 Point64 normal = face->getNormal();
2243#ifdef DEBUG_CONVEX_HULL
2244 printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
2245 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);
2246#endif
2247 int64_t origDot = face->origin.dot(normal);
2248 Point32 shiftedOrigin = face->origin + shift;
2249 int64_t shiftedDot = shiftedOrigin.dot(normal);
2250 btAssert(shiftedDot <= origDot);
2251 if (shiftedDot >= origDot)
2252 {
2253 return false;
2254 }
2255
2256 Edge* intersection = NULL;
2257
2258 Edge* startEdge = face->nearbyVertex->edges;
2259#ifdef DEBUG_CONVEX_HULL
2260 printf("Start edge is ");
2261 startEdge->print();
2262 printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot);
2263#endif
2264 Rational128 optDot = face->nearbyVertex->dot(normal);
2265 int cmp = optDot.compare(shiftedDot);
2266#ifdef SHOW_ITERATIONS
2267 int n = 0;
2268#endif
2269 if (cmp >= 0)
2270 {
2271 Edge* e = startEdge;
2272 do
2273 {
2274#ifdef SHOW_ITERATIONS
2275 n++;
2276#endif
2277 Rational128 dot = e->target->dot(normal);
2278 btAssert(dot.compare(origDot) <= 0);
2279#ifdef DEBUG_CONVEX_HULL
2280 printf("Moving downwards, edge is ");
2281 e->print();
2282 printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
2283#endif
2284 if (dot.compare(optDot) < 0)
2285 {
2286 int c = dot.compare(shiftedDot);
2287 optDot = dot;
2288 e = e->reverse;
2289 startEdge = e;
2290 if (c < 0)
2291 {
2292 intersection = e;
2293 break;
2294 }
2295 cmp = c;
2296 }
2297 e = e->prev;
2298 } while (e != startEdge);
2299
2300 if (!intersection)
2301 {
2302 return false;
2303 }
2304 }
2305 else
2306 {
2307 Edge* e = startEdge;
2308 do
2309 {
2310#ifdef SHOW_ITERATIONS
2311 n++;
2312#endif
2313 Rational128 dot = e->target->dot(normal);
2314 btAssert(dot.compare(origDot) <= 0);
2315#ifdef DEBUG_CONVEX_HULL
2316 printf("Moving upwards, edge is ");
2317 e->print();
2318 printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
2319#endif
2320 if (dot.compare(optDot) > 0)
2321 {
2322 cmp = dot.compare(shiftedDot);
2323 if (cmp >= 0)
2324 {
2325 intersection = e;
2326 break;
2327 }
2328 optDot = dot;
2329 e = e->reverse;
2330 startEdge = e;
2331 }
2332 e = e->prev;
2333 } while (e != startEdge);
2334
2335 if (!intersection)
2336 {
2337 return true;
2338 }
2339 }
2340
2341#ifdef SHOW_ITERATIONS
2342 printf("Needed %d iterations to find initial intersection\n", n);
2343#endif
2344
2345 if (cmp == 0)
2346 {
2347 Edge* e = intersection->reverse->next;
2348#ifdef SHOW_ITERATIONS
2349 n = 0;
2350#endif
2351 while (e->target->dot(normal).compare(shiftedDot) <= 0)
2352 {
2353#ifdef SHOW_ITERATIONS
2354 n++;
2355#endif
2356 e = e->next;
2357 if (e == intersection->reverse)
2358 {
2359 return true;
2360 }
2361#ifdef DEBUG_CONVEX_HULL
2362 printf("Checking for outwards edge, current edge is ");
2363 e->print();
2364 printf("\n");
2365#endif
2366 }
2367#ifdef SHOW_ITERATIONS
2368 printf("Needed %d iterations to check for complete containment\n", n);
2369#endif
2370 }
2371
2372 Edge* firstIntersection = NULL;
2373 Edge* faceEdge = NULL;
2374 Edge* firstFaceEdge = NULL;
2375
2376#ifdef SHOW_ITERATIONS
2377 int m = 0;
2378#endif
2379 while (true)
2380 {
2381#ifdef SHOW_ITERATIONS
2382 m++;
2383#endif
2384#ifdef DEBUG_CONVEX_HULL
2385 printf("Intersecting edge is ");
2386 intersection->print();
2387 printf("\n");
2388#endif
2389 if (cmp == 0)
2390 {
2391 Edge* e = intersection->reverse->next;
2392 startEdge = e;
2393#ifdef SHOW_ITERATIONS
2394 n = 0;
2395#endif
2396 while (true)
2397 {
2398#ifdef SHOW_ITERATIONS
2399 n++;
2400#endif
2401 if (e->target->dot(normal).compare(shiftedDot) >= 0)
2402 {
2403 break;
2404 }
2405 intersection = e->reverse;
2406 e = e->next;
2407 if (e == startEdge)
2408 {
2409 return true;
2410 }
2411 }
2412#ifdef SHOW_ITERATIONS
2413 printf("Needed %d iterations to advance intersection\n", n);
2414#endif
2415 }
2416
2417#ifdef DEBUG_CONVEX_HULL
2418 printf("Advanced intersecting edge to ");
2419 intersection->print();
2420 printf(", cmp = %d\n", cmp);
2421#endif
2422
2423 if (!firstIntersection)
2424 {
2425 firstIntersection = intersection;
2426 }
2427 else if (intersection == firstIntersection)
2428 {
2429 break;
2430 }
2431
2432 int prevCmp = cmp;
2433 Edge* prevIntersection = intersection;
2434 Edge* prevFaceEdge = faceEdge;
2435
2436 Edge* e = intersection->reverse;
2437#ifdef SHOW_ITERATIONS
2438 n = 0;
2439#endif
2440 while (true)
2441 {
2442#ifdef SHOW_ITERATIONS
2443 n++;
2444#endif
2445 e = e->reverse->prev;
2446 btAssert(e != intersection->reverse);
2447 cmp = e->target->dot(normal).compare(shiftedDot);
2448#ifdef DEBUG_CONVEX_HULL
2449 printf("Testing edge ");
2450 e->print();
2451 printf(" -> cmp = %d\n", cmp);
2452#endif
2453 if (cmp >= 0)
2454 {
2455 intersection = e;
2456 break;
2457 }
2458 }
2459#ifdef SHOW_ITERATIONS
2460 printf("Needed %d iterations to find other intersection of face\n", n);
2461#endif
2462
2463 if (cmp > 0)
2464 {
2465 Vertex* removed = intersection->target;
2466 e = intersection->reverse;
2467 if (e->prev == e)
2468 {
2469 removed->edges = NULL;
2470 }
2471 else
2472 {
2473 removed->edges = e->prev;
2474 e->prev->link(e->next);
2475 e->link(e);
2476 }
2477#ifdef DEBUG_CONVEX_HULL
2478 printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2479#endif
2480
2481 Point64 n0 = intersection->face->getNormal();
2482 Point64 n1 = intersection->reverse->face->getNormal();
2483 int64_t m00 = face->dir0.dot(n0);
2484 int64_t m01 = face->dir1.dot(n0);
2485 int64_t m10 = face->dir0.dot(n1);
2486 int64_t m11 = face->dir1.dot(n1);
2487 int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
2488 int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
2489 Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
2490 btAssert(det.getSign() != 0);
2491 Vertex* v = vertexPool.newObject();
2492 v->point.index = -1;
2493 v->copy = -1;
2494 v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01)
2495 + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
2496 Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01)
2497 + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
2498 Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01)
2499 + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
2500 det);
2501 v->point.x = (int32_t) v->point128.xvalue();
2502 v->point.y = (int32_t) v->point128.yvalue();
2503 v->point.z = (int32_t) v->point128.zvalue();
2504 intersection->target = v;
2505 v->edges = e;
2506
2507 stack.push_back(v);
2508 stack.push_back(removed);
2509 stack.push_back(NULL);
2510 }
2511
2512 if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target))
2513 {
2514 faceEdge = newEdgePair(prevIntersection->target, intersection->target);
2515 if (prevCmp == 0)
2516 {
2517 faceEdge->link(prevIntersection->reverse->next);
2518 }
2519 if ((prevCmp == 0) || prevFaceEdge)
2520 {
2521 prevIntersection->reverse->link(faceEdge);
2522 }
2523 if (cmp == 0)
2524 {
2525 intersection->reverse->prev->link(faceEdge->reverse);
2526 }
2527 faceEdge->reverse->link(intersection->reverse);
2528 }
2529 else
2530 {
2531 faceEdge = prevIntersection->reverse->next;
2532 }
2533
2534 if (prevFaceEdge)
2535 {
2536 if (prevCmp > 0)
2537 {
2538 faceEdge->link(prevFaceEdge->reverse);
2539 }
2540 else if (faceEdge != prevFaceEdge->reverse)
2541 {
2542 stack.push_back(prevFaceEdge->target);
2543 while (faceEdge->next != prevFaceEdge->reverse)
2544 {
2545 Vertex* removed = faceEdge->next->target;
2546 removeEdgePair(faceEdge->next);
2547 stack.push_back(removed);
2548#ifdef DEBUG_CONVEX_HULL
2549 printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2550#endif
2551 }
2552 stack.push_back(NULL);
2553 }
2554 }
2555 faceEdge->face = face;
2556 faceEdge->reverse->face = intersection->face;
2557
2558 if (!firstFaceEdge)
2559 {
2560 firstFaceEdge = faceEdge;
2561 }
2562 }
2563#ifdef SHOW_ITERATIONS
2564 printf("Needed %d iterations to process all intersections\n", m);
2565#endif
2566
2567 if (cmp > 0)
2568 {
2569 firstFaceEdge->reverse->target = faceEdge->target;
2570 firstIntersection->reverse->link(firstFaceEdge);
2571 firstFaceEdge->link(faceEdge->reverse);
2572 }
2573 else if (firstFaceEdge != faceEdge->reverse)
2574 {
2575 stack.push_back(faceEdge->target);
2576 while (firstFaceEdge->next != faceEdge->reverse)
2577 {
2578 Vertex* removed = firstFaceEdge->next->target;
2579 removeEdgePair(firstFaceEdge->next);
2580 stack.push_back(removed);
2581#ifdef DEBUG_CONVEX_HULL
2582 printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2583#endif
2584 }
2585 stack.push_back(NULL);
2586 }
2587
2588 btAssert(stack.size() > 0);
2589 vertexList = stack[0];
2590
2591#ifdef DEBUG_CONVEX_HULL
2592 printf("Removing part\n");
2593#endif
2594#ifdef SHOW_ITERATIONS
2595 n = 0;
2596#endif
2597 int pos = 0;
2598 while (pos < stack.size())
2599 {
2600 int end = stack.size();
2601 while (pos < end)
2602 {
2603 Vertex* kept = stack[pos++];
2604#ifdef DEBUG_CONVEX_HULL
2605 kept->print();
2606#endif
2607 bool deeper = false;
2608 Vertex* removed;
2609 while ((removed = stack[pos++]) != NULL)
2610 {
2611#ifdef SHOW_ITERATIONS
2612 n++;
2613#endif
2614 kept->receiveNearbyFaces(removed);
2615 while (removed->edges)
2616 {
2617 if (!deeper)
2618 {
2619 deeper = true;
2620 stack.push_back(kept);
2621 }
2622 stack.push_back(removed->edges->target);
2623 removeEdgePair(removed->edges);
2624 }
2625 }
2626 if (deeper)
2627 {
2628 stack.push_back(NULL);
2629 }
2630 }
2631 }
2632#ifdef SHOW_ITERATIONS
2633 printf("Needed %d iterations to remove part\n", n);
2634#endif
2635
2636 stack.resize(0);
2637 face->origin = shiftedOrigin;
2638
2639 return true;
2640}
2641
2642
2644{
2645 int index = vertex->copy;
2646 if (index < 0)
2647 {
2648 index = vertices.size();
2649 vertex->copy = index;
2650 vertices.push_back(vertex);
2651#ifdef DEBUG_CONVEX_HULL
2652 printf("Vertex %d gets index *%d\n", vertex->point.index, index);
2653#endif
2654 }
2655 return index;
2656}
2657
2658btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
2659{
2660 if (count <= 0)
2661 {
2662 vertices.clear();
2663 edges.clear();
2664 faces.clear();
2665 return 0;
2666 }
2667
2669 hull.compute(coords, doubleCoords, stride, count);
2670
2671 btScalar shift = 0;
2672 if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0))
2673 {
2674 vertices.clear();
2675 edges.clear();
2676 faces.clear();
2677 return shift;
2678 }
2679
2680 vertices.resize(0);
2681 edges.resize(0);
2682 faces.resize(0);
2683
2685 getVertexCopy(hull.vertexList, oldVertices);
2686 int copied = 0;
2687 while (copied < oldVertices.size())
2688 {
2689 btConvexHullInternal::Vertex* v = oldVertices[copied];
2690 vertices.push_back(hull.getCoordinates(v));
2691 btConvexHullInternal::Edge* firstEdge = v->edges;
2692 if (firstEdge)
2693 {
2694 int firstCopy = -1;
2695 int prevCopy = -1;
2696 btConvexHullInternal::Edge* e = firstEdge;
2697 do
2698 {
2699 if (e->copy < 0)
2700 {
2701 int s = edges.size();
2702 edges.push_back(Edge());
2703 edges.push_back(Edge());
2704 Edge* c = &edges[s];
2705 Edge* r = &edges[s + 1];
2706 e->copy = s;
2707 e->reverse->copy = s + 1;
2708 c->reverse = 1;
2709 r->reverse = -1;
2710 c->targetVertex = getVertexCopy(e->target, oldVertices);
2711 r->targetVertex = copied;
2712#ifdef DEBUG_CONVEX_HULL
2713 printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex());
2714#endif
2715 }
2716 if (prevCopy >= 0)
2717 {
2718 edges[e->copy].next = prevCopy - e->copy;
2719 }
2720 else
2721 {
2722 firstCopy = e->copy;
2723 }
2724 prevCopy = e->copy;
2725 e = e->next;
2726 } while (e != firstEdge);
2727 edges[firstCopy].next = prevCopy - firstCopy;
2728 }
2729 copied++;
2730 }
2731
2732 for (int i = 0; i < copied; i++)
2733 {
2734 btConvexHullInternal::Vertex* v = oldVertices[i];
2735 btConvexHullInternal::Edge* firstEdge = v->edges;
2736 if (firstEdge)
2737 {
2738 btConvexHullInternal::Edge* e = firstEdge;
2739 do
2740 {
2741 if (e->copy >= 0)
2742 {
2743#ifdef DEBUG_CONVEX_HULL
2744 printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
2745#endif
2746 faces.push_back(e->copy);
2748 do
2749 {
2750#ifdef DEBUG_CONVEX_HULL
2751 printf(" Face *%d\n", edges[f->copy].getTargetVertex());
2752#endif
2753 f->copy = -1;
2754 f = f->reverse->prev;
2755 } while (f != e);
2756 }
2757 e = e->next;
2758 } while (e != firstEdge);
2759 }
2760 }
2761
2762 return shift;
2763}
2764
2765
2766
2767
2768
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
long long int int64_t
int int32_t
unsigned int uint32_t
static int getVertexCopy(btConvexHullInternal::Vertex *vertex, btAlignedObjectArray< btConvexHullInternal::Vertex * > &vertices)
unsigned long long int uint64_t
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:23
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define SIMD_INFINITY
Definition: btScalar.h:522
btScalar btAtan(btScalar x)
Definition: btScalar.h:495
void btSwap(T &a, T &b)
Definition: btScalar.h:621
#define btAssert(x)
Definition: btScalar.h:131
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
void quickSort(const L &CompareFunc)
void push_back(const T &_Val)
btScalar compute(const void *coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
static uint64_t high(Int128 value)
static void shlHalf(uint64_t &value)
static void shlHalf(Int128 &value)
static uint64_t low(Int128 value)
static uint32_t low(uint64_t value)
static void mul(UWord a, UWord b, UWord &resLow, UWord &resHigh)
static uint32_t high(uint64_t value)
static uint64_t mul(uint32_t a, uint32_t b)
static Int128 mul(uint64_t a, uint64_t b)
void init(Vertex *a, Vertex *b, Vertex *c)
int ucmp(const Int128 &b) const
Int128 operator+(const Int128 &b) const
Int128 operator*(int64_t b) const
Int128(uint64_t low, uint64_t high)
bool operator<(const Int128 &b) const
Int128 operator-(const Int128 &b) const
static Int128 mul(int64_t a, int64_t b)
Int128 & operator+=(const Int128 &b)
int64_t dot(const Point64 &b) const
Point32(int32_t x, int32_t y, int32_t z)
Point32 operator-(const Point32 &b) const
int64_t dot(const Point32 &b) const
bool operator!=(const Point32 &b) const
Point64 cross(const Point64 &b) const
Point32 operator+(const Point32 &b) const
Point64 cross(const Point32 &b) const
bool operator==(const Point32 &b) const
Point64(int64_t x, int64_t y, int64_t z)
int64_t dot(const Point64 &b) const
PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator)
Rational128(const Int128 &numerator, const Int128 &denominator)
int compare(const Rational128 &b) const
int compare(const Rational64 &b) const
Rational64(int64_t numerator, int64_t denominator)
Point32 operator-(const Vertex &b) const
Rational128 dot(const Point64 &b) const
bool mergeProjection(IntermediateHull &h0, IntermediateHull &h1, Vertex *&c0, Vertex *&c1)
btVector3 toBtVector(const Point32 &v)
void computeInternal(int start, int end, IntermediateHull &result)
void findEdgeForCoplanarFaces(Vertex *c0, Vertex *c1, Edge *&e0, Edge *&e1, Vertex *stop0, Vertex *stop1)
void compute(const void *coords, bool doubleCoords, int stride, int count)
bool shiftFace(Face *face, btScalar amount, btAlignedObjectArray< Vertex * > stack)
btVector3 getCoordinates(const Vertex *v)
btAlignedObjectArray< Vertex * > originalVertices
btScalar shrink(btScalar amount, btScalar clampAmount)
Edge * findMaxAngle(bool ccw, const Vertex *start, const Point32 &s, const Point64 &rxs, const Point64 &sxrxs, Rational64 &minCot)
static Orientation getOrientation(const Edge *prev, const Edge *next, const Point32 &s, const Point32 &t)
Edge * newEdgePair(Vertex *from, Vertex *to)
void merge(IntermediateHull &h0, IntermediateHull &h1)
void removeEdgePair(Edge *edge)
btVector3 getBtNormal(Face *face)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:621
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
int minAxis() const
Return the axis with the smallest value Note return values are 0,1,2 for x, y, or z.
Definition: btVector3.h:480
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:964
int maxAxis() const
Return the axis with the largest value Note return values are 0,1,2 for x, y, or z.
Definition: btVector3.h:487