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
Point64(int64_t x,int64_t y,int64_t z)63 Point64(int64_t x, int64_t y, int64_t z): x(x), y(y), z(z)
64 {
65 }
66
isZero()67 bool isZero()
68 {
69 return (x == 0) && (y == 0) && (z == 0);
70 }
71
dot(const Point64 & b) const72 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
Point32()86 Point32()
87 {
88 }
89
Point32(int32_t x,int32_t y,int32_t z)90 Point32(int32_t x, int32_t y, int32_t z): x(x), y(y), z(z), index(-1)
91 {
92 }
93
operator ==(const Point32 & b) const94 bool operator==(const Point32& b) const
95 {
96 return (x == b.x) && (y == b.y) && (z == b.z);
97 }
98
operator !=(const Point32 & b) const99 bool operator!=(const Point32& b) const
100 {
101 return (x != b.x) || (y != b.y) || (z != b.z);
102 }
103
isZero()104 bool isZero()
105 {
106 return (x == 0) && (y == 0) && (z == 0);
107 }
108
cross(const Point32 & b) const109 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
cross(const Point64 & b) const114 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
dot(const Point32 & b) const119 int64_t dot(const Point32& b) const
120 {
121 return x * b.x + y * b.y + z * b.z;
122 }
123
dot(const Point64 & b) const124 int64_t dot(const Point64& b) const
125 {
126 return x * b.x + y * b.y + z * b.z;
127 }
128
operator +(const Point32 & b) const129 Point32 operator+(const Point32& b) const
130 {
131 return Point32(x + b.x, y + b.y, z + b.z);
132 }
133
operator -(const Point32 & b) const134 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
Int128()146 Int128()
147 {
148 }
149
Int128(uint64_t low,uint64_t high)150 Int128(uint64_t low, uint64_t high): low(low), high(high)
151 {
152 }
153
Int128(uint64_t low)154 Int128(uint64_t low): low(low), high(0)
155 {
156 }
157
Int128(int64_t value)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
operator -() const166 Int128 operator-() const
167 {
168 return Int128((uint64_t) -(int64_t)low, ~high + (low == 0));
169 }
170
operator +(const Int128 & b) const171 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
operator -(const Int128 & b) const187 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
operator +=(const Int128 & b)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
operator ++()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
toScalar() const233 btScalar toScalar() const
234 {
235 return ((int64_t) high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low)
236 : -(-*this).toScalar();
237 }
238
getSign() const239 int getSign() const
240 {
241 return ((int64_t) high < 0) ? -1 : (high || low) ? 1 : 0;
242 }
243
operator <(const Int128 & b) const244 bool operator<(const Int128& b) const
245 {
246 return (high < b.high) || ((high == b.high) && (low < b.low));
247 }
248
ucmp(const Int128 & b) const249 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 numerator;
276 uint64_t denominator;
277 int sign;
278
279 public:
Rational64(int64_t numerator,int64_t denominator)280 Rational64(int64_t numerator, int64_t denominator)
281 {
282 if (numerator > 0)
283 {
284 sign = 1;
285 this->numerator = (uint64_t) numerator;
286 }
287 else if (numerator < 0)
288 {
289 sign = -1;
290 this->numerator = (uint64_t) -numerator;
291 }
292 else
293 {
294 sign = 0;
295 this->numerator = 0;
296 }
297 if (denominator > 0)
298 {
299 this->denominator = (uint64_t) denominator;
300 }
301 else if (denominator < 0)
302 {
303 sign = -sign;
304 this->denominator = (uint64_t) -denominator;
305 }
306 else
307 {
308 this->denominator = 0;
309 }
310 }
311
isNegativeInfinity() const312 bool isNegativeInfinity() const
313 {
314 return (sign < 0) && (denominator == 0);
315 }
316
isNaN() const317 bool isNaN() const
318 {
319 return (sign == 0) && (denominator == 0);
320 }
321
322 int compare(const Rational64& b) const;
323
toScalar() const324 btScalar toScalar() const
325 {
326 return sign * ((denominator == 0) ? SIMD_INFINITY : (btScalar) numerator / 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:
Rational128(int64_t value)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
Rational128(const Int128 & numerator,const Int128 & denominator)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
toScalar() const389 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
PointR128()403 PointR128()
404 {
405 }
406
PointR128(Int128 x,Int128 y,Int128 z,Int128 denominator)407 PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator): x(x), y(y), z(z), denominator(denominator)
408 {
409 }
410
xvalue() const411 btScalar xvalue() const
412 {
413 return x.toScalar() / denominator.toScalar();
414 }
415
yvalue() const416 btScalar yvalue() const
417 {
418 return y.toScalar() / denominator.toScalar();
419 }
420
zvalue() const421 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
Vertex()443 Vertex(): next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1)
444 {
445 }
446
447 #ifdef DEBUG_CONVEX_HULL
print()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
operator -(const Vertex & b) const456 Point32 operator-(const Vertex& b) const
457 {
458 return point - b.point;
459 }
460
dot(const Point64 & b) const461 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
xvalue() const467 btScalar xvalue() const
468 {
469 return (point.index >= 0) ? btScalar(point.x) : point128.xvalue();
470 }
471
yvalue() const472 btScalar yvalue() const
473 {
474 return (point.index >= 0) ? btScalar(point.y) : point128.yvalue();
475 }
476
zvalue() const477 btScalar zvalue() const
478 {
479 return (point.index >= 0) ? btScalar(point.z) : point128.zvalue();
480 }
481
receiveNearbyFaces(Vertex * src)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
~Edge()517 ~Edge()
518 {
519 next = NULL;
520 prev = NULL;
521 reverse = NULL;
522 target = NULL;
523 face = NULL;
524 }
525
link(Edge * n)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
print()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
Face()552 Face(): next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL)
553 {
554 }
555
init(Vertex * a,Vertex * b,Vertex * c)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
getNormal()573 Point64 getNormal()
574 {
575 return dir0.cross(dir1);
576 }
577 };
578
579 template<typename UWord, typename UHWord> class DMul
580 {
581 private:
high(uint64_t value)582 static uint32_t high(uint64_t value)
583 {
584 return (uint32_t) (value >> 32);
585 }
586
low(uint64_t value)587 static uint32_t low(uint64_t value)
588 {
589 return (uint32_t) value;
590 }
591
mul(uint32_t a,uint32_t b)592 static uint64_t mul(uint32_t a, uint32_t b)
593 {
594 return (uint64_t) a * (uint64_t) b;
595 }
596
shlHalf(uint64_t & value)597 static void shlHalf(uint64_t& value)
598 {
599 value <<= 32;
600 }
601
high(Int128 value)602 static uint64_t high(Int128 value)
603 {
604 return value.high;
605 }
606
low(Int128 value)607 static uint64_t low(Int128 value)
608 {
609 return value.low;
610 }
611
mul(uint64_t a,uint64_t b)612 static Int128 mul(uint64_t a, uint64_t b)
613 {
614 return Int128::mul(a, b);
615 }
616
shlHalf(Int128 & value)617 static void shlHalf(Int128& value)
618 {
619 value.high = value.low;
620 value.low = 0;
621 }
622
623 public:
624
mul(UWord a,UWord b,UWord & resLow,UWord & resHigh)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
IntermediateHull()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
PoolArray(int size)674 PoolArray(int size): size(size), next(NULL)
675 {
676 array = (T*) btAlignedAlloc(sizeof(T) * size, 16);
677 }
678
~PoolArray()679 ~PoolArray()
680 {
681 btAlignedFree(array);
682 }
683
init()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:
Pool()704 Pool(): arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256)
705 {
706 }
707
~Pool()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
reset()719 void reset()
720 {
721 nextArray = arrays;
722 freeObjects = NULL;
723 }
724
setArraySize(int arraySize)725 void setArraySize(int arraySize)
726 {
727 this->arraySize = arraySize;
728 }
729
newObject()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
freeObject(T * object)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
removeEdgePair(Edge * edge)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
operator *(int64_t b) const838 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
mul(int64_t a,int64_t b)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
mul(uint64_t a,uint64_t b)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
compare(const Rational64 & b) const896 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(numerator, b.denominator).ucmp(Int128::mul(denominator, b.numerator));
936
937 #endif
938 }
939
compare(const Rational128 & b) const940 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
compare(int64_t b) const967 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
newEdgePair(Vertex * from,Vertex * to)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
mergeProjection(IntermediateHull & h0,IntermediateHull & h1,Vertex * & c0,Vertex * & c1)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
computeInternal(int start,int end,IntermediateHull & result)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
print()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
printGraph()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
getOrientation(const Edge * prev,const Edge * next,const Point32 & s,const Point32 & t)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
findMaxAngle(bool ccw,const Vertex * start,const Point32 & s,const Point64 & rxs,const Point64 & sxrxs,Rational64 & minCot)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
findEdgeForCoplanarFaces(Vertex * c0,Vertex * c1,Edge * & e0,Edge * & e1,Vertex * stop0,Vertex * stop1)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
merge(IntermediateHull & h0,IntermediateHull & h1)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
pointCmp(const btConvexHullInternal::Point32 & p,const btConvexHullInternal::Point32 & q)1935 static bool pointCmp(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q)
1936 {
1937 return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
1938 }
1939
compute(const void * coords,bool doubleCoords,int stride,int count)1940 void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
1941 {
1942 btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30));
1943 const char* ptr = (const char*) coords;
1944 if (doubleCoords)
1945 {
1946 for (int i = 0; i < count; i++)
1947 {
1948 const double* v = (const double*) ptr;
1949 btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
1950 ptr += stride;
1951 min.setMin(p);
1952 max.setMax(p);
1953 }
1954 }
1955 else
1956 {
1957 for (int i = 0; i < count; i++)
1958 {
1959 const float* v = (const float*) ptr;
1960 btVector3 p(v[0], v[1], v[2]);
1961 ptr += stride;
1962 min.setMin(p);
1963 max.setMax(p);
1964 }
1965 }
1966
1967 btVector3 s = max - min;
1968 maxAxis = s.maxAxis();
1969 minAxis = s.minAxis();
1970 if (minAxis == maxAxis)
1971 {
1972 minAxis = (maxAxis + 1) % 3;
1973 }
1974 medAxis = 3 - maxAxis - minAxis;
1975
1976 s /= btScalar(10216);
1977 if (((medAxis + 1) % 3) != maxAxis)
1978 {
1979 s *= -1;
1980 }
1981 scaling = s;
1982
1983 if (s[0] != 0)
1984 {
1985 s[0] = btScalar(1) / s[0];
1986 }
1987 if (s[1] != 0)
1988 {
1989 s[1] = btScalar(1) / s[1];
1990 }
1991 if (s[2] != 0)
1992 {
1993 s[2] = btScalar(1) / s[2];
1994 }
1995
1996 center = (min + max) * btScalar(0.5);
1997
1998 btAlignedObjectArray<Point32> points;
1999 points.resize(count);
2000 ptr = (const char*) coords;
2001 if (doubleCoords)
2002 {
2003 for (int i = 0; i < count; i++)
2004 {
2005 const double* v = (const double*) ptr;
2006 btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
2007 ptr += stride;
2008 p = (p - center) * s;
2009 points[i].x = (int32_t) p[medAxis];
2010 points[i].y = (int32_t) p[maxAxis];
2011 points[i].z = (int32_t) p[minAxis];
2012 points[i].index = i;
2013 }
2014 }
2015 else
2016 {
2017 for (int i = 0; i < count; i++)
2018 {
2019 const float* v = (const float*) ptr;
2020 btVector3 p(v[0], v[1], v[2]);
2021 ptr += stride;
2022 p = (p - center) * s;
2023 points[i].x = (int32_t) p[medAxis];
2024 points[i].y = (int32_t) p[maxAxis];
2025 points[i].z = (int32_t) p[minAxis];
2026 points[i].index = i;
2027 }
2028 }
2029 points.quickSort(pointCmp);
2030
2031 vertexPool.reset();
2032 vertexPool.setArraySize(count);
2033 originalVertices.resize(count);
2034 for (int i = 0; i < count; i++)
2035 {
2036 Vertex* v = vertexPool.newObject();
2037 v->edges = NULL;
2038 v->point = points[i];
2039 v->copy = -1;
2040 originalVertices[i] = v;
2041 }
2042
2043 points.clear();
2044
2045 edgePool.reset();
2046 edgePool.setArraySize(6 * count);
2047
2048 usedEdgePairs = 0;
2049 maxUsedEdgePairs = 0;
2050
2051 mergeStamp = -3;
2052
2053 IntermediateHull hull;
2054 computeInternal(0, count, hull);
2055 vertexList = hull.minXy;
2056 #ifdef DEBUG_CONVEX_HULL
2057 printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count);
2058 #endif
2059 }
2060
toBtVector(const Point32 & v)2061 btVector3 btConvexHullInternal::toBtVector(const Point32& v)
2062 {
2063 btVector3 p;
2064 p[medAxis] = btScalar(v.x);
2065 p[maxAxis] = btScalar(v.y);
2066 p[minAxis] = btScalar(v.z);
2067 return p * scaling;
2068 }
2069
getBtNormal(Face * face)2070 btVector3 btConvexHullInternal::getBtNormal(Face* face)
2071 {
2072 return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized();
2073 }
2074
getCoordinates(const Vertex * v)2075 btVector3 btConvexHullInternal::getCoordinates(const Vertex* v)
2076 {
2077 btVector3 p;
2078 p[medAxis] = v->xvalue();
2079 p[maxAxis] = v->yvalue();
2080 p[minAxis] = v->zvalue();
2081 return p * scaling + center;
2082 }
2083
shrink(btScalar amount,btScalar clampAmount)2084 btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
2085 {
2086 if (!vertexList)
2087 {
2088 return 0;
2089 }
2090 int stamp = --mergeStamp;
2091 btAlignedObjectArray<Vertex*> stack;
2092 vertexList->copy = stamp;
2093 stack.push_back(vertexList);
2094 btAlignedObjectArray<Face*> faces;
2095
2096 Point32 ref = vertexList->point;
2097 Int128 hullCenterX(0, 0);
2098 Int128 hullCenterY(0, 0);
2099 Int128 hullCenterZ(0, 0);
2100 Int128 volume(0, 0);
2101
2102 while (stack.size() > 0)
2103 {
2104 Vertex* v = stack[stack.size() - 1];
2105 stack.pop_back();
2106 Edge* e = v->edges;
2107 if (e)
2108 {
2109 do
2110 {
2111 if (e->target->copy != stamp)
2112 {
2113 e->target->copy = stamp;
2114 stack.push_back(e->target);
2115 }
2116 if (e->copy != stamp)
2117 {
2118 Face* face = facePool.newObject();
2119 face->init(e->target, e->reverse->prev->target, v);
2120 faces.push_back(face);
2121 Edge* f = e;
2122
2123 Vertex* a = NULL;
2124 Vertex* b = NULL;
2125 do
2126 {
2127 if (a && b)
2128 {
2129 int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
2130 btAssert(vol >= 0);
2131 Point32 c = v->point + a->point + b->point + ref;
2132 hullCenterX += vol * c.x;
2133 hullCenterY += vol * c.y;
2134 hullCenterZ += vol * c.z;
2135 volume += vol;
2136 }
2137
2138 btAssert(f->copy != stamp);
2139 f->copy = stamp;
2140 f->face = face;
2141
2142 a = b;
2143 b = f->target;
2144
2145 f = f->reverse->prev;
2146 } while (f != e);
2147 }
2148 e = e->next;
2149 } while (e != v->edges);
2150 }
2151 }
2152
2153 if (volume.getSign() <= 0)
2154 {
2155 return 0;
2156 }
2157
2158 btVector3 hullCenter;
2159 hullCenter[medAxis] = hullCenterX.toScalar();
2160 hullCenter[maxAxis] = hullCenterY.toScalar();
2161 hullCenter[minAxis] = hullCenterZ.toScalar();
2162 hullCenter /= 4 * volume.toScalar();
2163 hullCenter *= scaling;
2164
2165 int faceCount = faces.size();
2166
2167 if (clampAmount > 0)
2168 {
2169 btScalar minDist = SIMD_INFINITY;
2170 for (int i = 0; i < faceCount; i++)
2171 {
2172 btVector3 normal = getBtNormal(faces[i]);
2173 btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
2174 if (dist < minDist)
2175 {
2176 minDist = dist;
2177 }
2178 }
2179
2180 if (minDist <= 0)
2181 {
2182 return 0;
2183 }
2184
2185 amount = btMin(amount, minDist * clampAmount);
2186 }
2187
2188 unsigned int seed = 243703;
2189 for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
2190 {
2191 btSwap(faces[i], faces[seed % faceCount]);
2192 }
2193
2194 for (int i = 0; i < faceCount; i++)
2195 {
2196 if (!shiftFace(faces[i], amount, stack))
2197 {
2198 return -amount;
2199 }
2200 }
2201
2202 return amount;
2203 }
2204
shiftFace(Face * face,btScalar amount,btAlignedObjectArray<Vertex * > stack)2205 bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack)
2206 {
2207 btVector3 origShift = getBtNormal(face) * -amount;
2208 if (scaling[0] != 0)
2209 {
2210 origShift[0] /= scaling[0];
2211 }
2212 if (scaling[1] != 0)
2213 {
2214 origShift[1] /= scaling[1];
2215 }
2216 if (scaling[2] != 0)
2217 {
2218 origShift[2] /= scaling[2];
2219 }
2220 Point32 shift((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]);
2221 if (shift.isZero())
2222 {
2223 return true;
2224 }
2225 Point64 normal = face->getNormal();
2226 #ifdef DEBUG_CONVEX_HULL
2227 printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
2228 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);
2229 #endif
2230 int64_t origDot = face->origin.dot(normal);
2231 Point32 shiftedOrigin = face->origin + shift;
2232 int64_t shiftedDot = shiftedOrigin.dot(normal);
2233 btAssert(shiftedDot <= origDot);
2234 if (shiftedDot >= origDot)
2235 {
2236 return false;
2237 }
2238
2239 Edge* intersection = NULL;
2240
2241 Edge* startEdge = face->nearbyVertex->edges;
2242 #ifdef DEBUG_CONVEX_HULL
2243 printf("Start edge is ");
2244 startEdge->print();
2245 printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot);
2246 #endif
2247 Rational128 optDot = face->nearbyVertex->dot(normal);
2248 int cmp = optDot.compare(shiftedDot);
2249 #ifdef SHOW_ITERATIONS
2250 int n = 0;
2251 #endif
2252 if (cmp >= 0)
2253 {
2254 Edge* e = startEdge;
2255 do
2256 {
2257 #ifdef SHOW_ITERATIONS
2258 n++;
2259 #endif
2260 Rational128 dot = e->target->dot(normal);
2261 btAssert(dot.compare(origDot) <= 0);
2262 #ifdef DEBUG_CONVEX_HULL
2263 printf("Moving downwards, edge is ");
2264 e->print();
2265 printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
2266 #endif
2267 if (dot.compare(optDot) < 0)
2268 {
2269 int c = dot.compare(shiftedDot);
2270 optDot = dot;
2271 e = e->reverse;
2272 startEdge = e;
2273 if (c < 0)
2274 {
2275 intersection = e;
2276 break;
2277 }
2278 cmp = c;
2279 }
2280 e = e->prev;
2281 } while (e != startEdge);
2282
2283 if (!intersection)
2284 {
2285 return false;
2286 }
2287 }
2288 else
2289 {
2290 Edge* e = startEdge;
2291 do
2292 {
2293 #ifdef SHOW_ITERATIONS
2294 n++;
2295 #endif
2296 Rational128 dot = e->target->dot(normal);
2297 btAssert(dot.compare(origDot) <= 0);
2298 #ifdef DEBUG_CONVEX_HULL
2299 printf("Moving upwards, edge is ");
2300 e->print();
2301 printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
2302 #endif
2303 if (dot.compare(optDot) > 0)
2304 {
2305 cmp = dot.compare(shiftedDot);
2306 if (cmp >= 0)
2307 {
2308 intersection = e;
2309 break;
2310 }
2311 optDot = dot;
2312 e = e->reverse;
2313 startEdge = e;
2314 }
2315 e = e->prev;
2316 } while (e != startEdge);
2317
2318 if (!intersection)
2319 {
2320 return true;
2321 }
2322 }
2323
2324 #ifdef SHOW_ITERATIONS
2325 printf("Needed %d iterations to find initial intersection\n", n);
2326 #endif
2327
2328 if (cmp == 0)
2329 {
2330 Edge* e = intersection->reverse->next;
2331 #ifdef SHOW_ITERATIONS
2332 n = 0;
2333 #endif
2334 while (e->target->dot(normal).compare(shiftedDot) <= 0)
2335 {
2336 #ifdef SHOW_ITERATIONS
2337 n++;
2338 #endif
2339 e = e->next;
2340 if (e == intersection->reverse)
2341 {
2342 return true;
2343 }
2344 #ifdef DEBUG_CONVEX_HULL
2345 printf("Checking for outwards edge, current edge is ");
2346 e->print();
2347 printf("\n");
2348 #endif
2349 }
2350 #ifdef SHOW_ITERATIONS
2351 printf("Needed %d iterations to check for complete containment\n", n);
2352 #endif
2353 }
2354
2355 Edge* firstIntersection = NULL;
2356 Edge* faceEdge = NULL;
2357 Edge* firstFaceEdge = NULL;
2358
2359 #ifdef SHOW_ITERATIONS
2360 int m = 0;
2361 #endif
2362 while (true)
2363 {
2364 #ifdef SHOW_ITERATIONS
2365 m++;
2366 #endif
2367 #ifdef DEBUG_CONVEX_HULL
2368 printf("Intersecting edge is ");
2369 intersection->print();
2370 printf("\n");
2371 #endif
2372 if (cmp == 0)
2373 {
2374 Edge* e = intersection->reverse->next;
2375 startEdge = e;
2376 #ifdef SHOW_ITERATIONS
2377 n = 0;
2378 #endif
2379 while (true)
2380 {
2381 #ifdef SHOW_ITERATIONS
2382 n++;
2383 #endif
2384 if (e->target->dot(normal).compare(shiftedDot) >= 0)
2385 {
2386 break;
2387 }
2388 intersection = e->reverse;
2389 e = e->next;
2390 if (e == startEdge)
2391 {
2392 return true;
2393 }
2394 }
2395 #ifdef SHOW_ITERATIONS
2396 printf("Needed %d iterations to advance intersection\n", n);
2397 #endif
2398 }
2399
2400 #ifdef DEBUG_CONVEX_HULL
2401 printf("Advanced intersecting edge to ");
2402 intersection->print();
2403 printf(", cmp = %d\n", cmp);
2404 #endif
2405
2406 if (!firstIntersection)
2407 {
2408 firstIntersection = intersection;
2409 }
2410 else if (intersection == firstIntersection)
2411 {
2412 break;
2413 }
2414
2415 int prevCmp = cmp;
2416 Edge* prevIntersection = intersection;
2417 Edge* prevFaceEdge = faceEdge;
2418
2419 Edge* e = intersection->reverse;
2420 #ifdef SHOW_ITERATIONS
2421 n = 0;
2422 #endif
2423 while (true)
2424 {
2425 #ifdef SHOW_ITERATIONS
2426 n++;
2427 #endif
2428 e = e->reverse->prev;
2429 btAssert(e != intersection->reverse);
2430 cmp = e->target->dot(normal).compare(shiftedDot);
2431 #ifdef DEBUG_CONVEX_HULL
2432 printf("Testing edge ");
2433 e->print();
2434 printf(" -> cmp = %d\n", cmp);
2435 #endif
2436 if (cmp >= 0)
2437 {
2438 intersection = e;
2439 break;
2440 }
2441 }
2442 #ifdef SHOW_ITERATIONS
2443 printf("Needed %d iterations to find other intersection of face\n", n);
2444 #endif
2445
2446 if (cmp > 0)
2447 {
2448 Vertex* removed = intersection->target;
2449 e = intersection->reverse;
2450 if (e->prev == e)
2451 {
2452 removed->edges = NULL;
2453 }
2454 else
2455 {
2456 removed->edges = e->prev;
2457 e->prev->link(e->next);
2458 e->link(e);
2459 }
2460 #ifdef DEBUG_CONVEX_HULL
2461 printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2462 #endif
2463
2464 Point64 n0 = intersection->face->getNormal();
2465 Point64 n1 = intersection->reverse->face->getNormal();
2466 int64_t m00 = face->dir0.dot(n0);
2467 int64_t m01 = face->dir1.dot(n0);
2468 int64_t m10 = face->dir0.dot(n1);
2469 int64_t m11 = face->dir1.dot(n1);
2470 int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
2471 int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
2472 Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
2473 btAssert(det.getSign() != 0);
2474 Vertex* v = vertexPool.newObject();
2475 v->point.index = -1;
2476 v->copy = -1;
2477 v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01)
2478 + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
2479 Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01)
2480 + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
2481 Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01)
2482 + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
2483 det);
2484 v->point.x = (int32_t) v->point128.xvalue();
2485 v->point.y = (int32_t) v->point128.yvalue();
2486 v->point.z = (int32_t) v->point128.zvalue();
2487 intersection->target = v;
2488 v->edges = e;
2489
2490 stack.push_back(v);
2491 stack.push_back(removed);
2492 stack.push_back(NULL);
2493 }
2494
2495 if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target))
2496 {
2497 faceEdge = newEdgePair(prevIntersection->target, intersection->target);
2498 if (prevCmp == 0)
2499 {
2500 faceEdge->link(prevIntersection->reverse->next);
2501 }
2502 if ((prevCmp == 0) || prevFaceEdge)
2503 {
2504 prevIntersection->reverse->link(faceEdge);
2505 }
2506 if (cmp == 0)
2507 {
2508 intersection->reverse->prev->link(faceEdge->reverse);
2509 }
2510 faceEdge->reverse->link(intersection->reverse);
2511 }
2512 else
2513 {
2514 faceEdge = prevIntersection->reverse->next;
2515 }
2516
2517 if (prevFaceEdge)
2518 {
2519 if (prevCmp > 0)
2520 {
2521 faceEdge->link(prevFaceEdge->reverse);
2522 }
2523 else if (faceEdge != prevFaceEdge->reverse)
2524 {
2525 stack.push_back(prevFaceEdge->target);
2526 while (faceEdge->next != prevFaceEdge->reverse)
2527 {
2528 Vertex* removed = faceEdge->next->target;
2529 removeEdgePair(faceEdge->next);
2530 stack.push_back(removed);
2531 #ifdef DEBUG_CONVEX_HULL
2532 printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2533 #endif
2534 }
2535 stack.push_back(NULL);
2536 }
2537 }
2538 faceEdge->face = face;
2539 faceEdge->reverse->face = intersection->face;
2540
2541 if (!firstFaceEdge)
2542 {
2543 firstFaceEdge = faceEdge;
2544 }
2545 }
2546 #ifdef SHOW_ITERATIONS
2547 printf("Needed %d iterations to process all intersections\n", m);
2548 #endif
2549
2550 if (cmp > 0)
2551 {
2552 firstFaceEdge->reverse->target = faceEdge->target;
2553 firstIntersection->reverse->link(firstFaceEdge);
2554 firstFaceEdge->link(faceEdge->reverse);
2555 }
2556 else if (firstFaceEdge != faceEdge->reverse)
2557 {
2558 stack.push_back(faceEdge->target);
2559 while (firstFaceEdge->next != faceEdge->reverse)
2560 {
2561 Vertex* removed = firstFaceEdge->next->target;
2562 removeEdgePair(firstFaceEdge->next);
2563 stack.push_back(removed);
2564 #ifdef DEBUG_CONVEX_HULL
2565 printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2566 #endif
2567 }
2568 stack.push_back(NULL);
2569 }
2570
2571 btAssert(stack.size() > 0);
2572 vertexList = stack[0];
2573
2574 #ifdef DEBUG_CONVEX_HULL
2575 printf("Removing part\n");
2576 #endif
2577 #ifdef SHOW_ITERATIONS
2578 n = 0;
2579 #endif
2580 int pos = 0;
2581 while (pos < stack.size())
2582 {
2583 int end = stack.size();
2584 while (pos < end)
2585 {
2586 Vertex* kept = stack[pos++];
2587 #ifdef DEBUG_CONVEX_HULL
2588 kept->print();
2589 #endif
2590 bool deeper = false;
2591 Vertex* removed;
2592 while ((removed = stack[pos++]) != NULL)
2593 {
2594 #ifdef SHOW_ITERATIONS
2595 n++;
2596 #endif
2597 kept->receiveNearbyFaces(removed);
2598 while (removed->edges)
2599 {
2600 if (!deeper)
2601 {
2602 deeper = true;
2603 stack.push_back(kept);
2604 }
2605 stack.push_back(removed->edges->target);
2606 removeEdgePair(removed->edges);
2607 }
2608 }
2609 if (deeper)
2610 {
2611 stack.push_back(NULL);
2612 }
2613 }
2614 }
2615 #ifdef SHOW_ITERATIONS
2616 printf("Needed %d iterations to remove part\n", n);
2617 #endif
2618
2619 stack.resize(0);
2620 face->origin = shiftedOrigin;
2621
2622 return true;
2623 }
2624
2625
getVertexCopy(btConvexHullInternal::Vertex * vertex,btAlignedObjectArray<btConvexHullInternal::Vertex * > & vertices)2626 static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices)
2627 {
2628 int index = vertex->copy;
2629 if (index < 0)
2630 {
2631 index = vertices.size();
2632 vertex->copy = index;
2633 vertices.push_back(vertex);
2634 #ifdef DEBUG_CONVEX_HULL
2635 printf("Vertex %d gets index *%d\n", vertex->point.index, index);
2636 #endif
2637 }
2638 return index;
2639 }
2640
compute(const void * coords,bool doubleCoords,int stride,int count,btScalar shrink,btScalar shrinkClamp)2641 btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
2642 {
2643 if (count <= 0)
2644 {
2645 vertices.clear();
2646 edges.clear();
2647 faces.clear();
2648 return 0;
2649 }
2650
2651 btConvexHullInternal hull;
2652 hull.compute(coords, doubleCoords, stride, count);
2653
2654 btScalar shift = 0;
2655 if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0))
2656 {
2657 vertices.clear();
2658 edges.clear();
2659 faces.clear();
2660 return shift;
2661 }
2662
2663 vertices.resize(0);
2664 edges.resize(0);
2665 faces.resize(0);
2666
2667 btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices;
2668 getVertexCopy(hull.vertexList, oldVertices);
2669 int copied = 0;
2670 while (copied < oldVertices.size())
2671 {
2672 btConvexHullInternal::Vertex* v = oldVertices[copied];
2673 vertices.push_back(hull.getCoordinates(v));
2674 btConvexHullInternal::Edge* firstEdge = v->edges;
2675 if (firstEdge)
2676 {
2677 int firstCopy = -1;
2678 int prevCopy = -1;
2679 btConvexHullInternal::Edge* e = firstEdge;
2680 do
2681 {
2682 if (e->copy < 0)
2683 {
2684 int s = edges.size();
2685 edges.push_back(Edge());
2686 edges.push_back(Edge());
2687 Edge* c = &edges[s];
2688 Edge* r = &edges[s + 1];
2689 e->copy = s;
2690 e->reverse->copy = s + 1;
2691 c->reverse = 1;
2692 r->reverse = -1;
2693 c->targetVertex = getVertexCopy(e->target, oldVertices);
2694 r->targetVertex = copied;
2695 #ifdef DEBUG_CONVEX_HULL
2696 printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex());
2697 #endif
2698 }
2699 if (prevCopy >= 0)
2700 {
2701 edges[e->copy].next = prevCopy - e->copy;
2702 }
2703 else
2704 {
2705 firstCopy = e->copy;
2706 }
2707 prevCopy = e->copy;
2708 e = e->next;
2709 } while (e != firstEdge);
2710 edges[firstCopy].next = prevCopy - firstCopy;
2711 }
2712 copied++;
2713 }
2714
2715 for (int i = 0; i < copied; i++)
2716 {
2717 btConvexHullInternal::Vertex* v = oldVertices[i];
2718 btConvexHullInternal::Edge* firstEdge = v->edges;
2719 if (firstEdge)
2720 {
2721 btConvexHullInternal::Edge* e = firstEdge;
2722 do
2723 {
2724 if (e->copy >= 0)
2725 {
2726 #ifdef DEBUG_CONVEX_HULL
2727 printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
2728 #endif
2729 faces.push_back(e->copy);
2730 btConvexHullInternal::Edge* f = e;
2731 do
2732 {
2733 #ifdef DEBUG_CONVEX_HULL
2734 printf(" Face *%d\n", edges[f->copy].getTargetVertex());
2735 #endif
2736 f->copy = -1;
2737 f = f->reverse->prev;
2738 } while (f != e);
2739 }
2740 e = e->next;
2741 } while (e != firstEdge);
2742 }
2743 }
2744
2745 return shift;
2746 }
2747
2748
2749
2750
2751
2752