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
1978 scaling = s;
1979 if (s[0] > 0)
1980 {
1981 s[0] = btScalar(1) / s[0];
1982 }
1983 if (s[1] > 0)
1984 {
1985 s[1] = btScalar(1) / s[1];
1986 }
1987 if (s[2] > 0)
1988 {
1989 s[2] = btScalar(1) / s[2];
1990 }
1991
1992 center = (min + max) * btScalar(0.5);
1993
1994 btAlignedObjectArray<Point32> points;
1995 points.resize(count);
1996 ptr = (const char*) coords;
1997 if (doubleCoords)
1998 {
1999 for (int i = 0; i < count; i++)
2000 {
2001 const double* v = (const double*) ptr;
2002 btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]);
2003 ptr += stride;
2004 p = (p - center) * s;
2005 points[i].x = (int32_t) p[medAxis];
2006 points[i].y = (int32_t) p[maxAxis];
2007 points[i].z = (int32_t) p[minAxis];
2008 points[i].index = i;
2009 }
2010 }
2011 else
2012 {
2013 for (int i = 0; i < count; i++)
2014 {
2015 const float* v = (const float*) ptr;
2016 btVector3 p(v[0], v[1], v[2]);
2017 ptr += stride;
2018 p = (p - center) * s;
2019 points[i].x = (int32_t) p[medAxis];
2020 points[i].y = (int32_t) p[maxAxis];
2021 points[i].z = (int32_t) p[minAxis];
2022 points[i].index = i;
2023 }
2024 }
2025 points.quickSort(pointCmp);
2026
2027 vertexPool.reset();
2028 vertexPool.setArraySize(count);
2029 originalVertices.resize(count);
2030 for (int i = 0; i < count; i++)
2031 {
2032 Vertex* v = vertexPool.newObject();
2033 v->edges = NULL;
2034 v->point = points[i];
2035 v->copy = -1;
2036 originalVertices[i] = v;
2037 }
2038
2039 points.clear();
2040
2041 edgePool.reset();
2042 edgePool.setArraySize(6 * count);
2043
2044 usedEdgePairs = 0;
2045 maxUsedEdgePairs = 0;
2046
2047 mergeStamp = -3;
2048
2049 IntermediateHull hull;
2050 computeInternal(0, count, hull);
2051 vertexList = hull.minXy;
2052 #ifdef DEBUG_CONVEX_HULL
2053 printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count);
2054 #endif
2055 }
2056
toBtVector(const Point32 & v)2057 btVector3 btConvexHullInternal::toBtVector(const Point32& v)
2058 {
2059 btVector3 p;
2060 p[medAxis] = btScalar(v.x);
2061 p[maxAxis] = btScalar(v.y);
2062 p[minAxis] = btScalar(v.z);
2063 return p * scaling;
2064 }
2065
getBtNormal(Face * face)2066 btVector3 btConvexHullInternal::getBtNormal(Face* face)
2067 {
2068 btVector3 normal = toBtVector(face->dir0).cross(toBtVector(face->dir1));
2069 normal /= ((medAxis + 1 == maxAxis) || (medAxis - 2 == maxAxis)) ? normal.length() : -normal.length();
2070 return normal;
2071 }
2072
getCoordinates(const Vertex * v)2073 btVector3 btConvexHullInternal::getCoordinates(const Vertex* v)
2074 {
2075 btVector3 p;
2076 p[medAxis] = v->xvalue();
2077 p[maxAxis] = v->yvalue();
2078 p[minAxis] = v->zvalue();
2079 return p * scaling + center;
2080 }
2081
shrink(btScalar amount,btScalar clampAmount)2082 btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
2083 {
2084 if (!vertexList)
2085 {
2086 return 0;
2087 }
2088 int stamp = --mergeStamp;
2089 btAlignedObjectArray<Vertex*> stack;
2090 vertexList->copy = stamp;
2091 stack.push_back(vertexList);
2092 btAlignedObjectArray<Face*> faces;
2093
2094 Point32 ref = vertexList->point;
2095 Int128 hullCenterX(0, 0);
2096 Int128 hullCenterY(0, 0);
2097 Int128 hullCenterZ(0, 0);
2098 Int128 volume(0, 0);
2099
2100 while (stack.size() > 0)
2101 {
2102 Vertex* v = stack[stack.size() - 1];
2103 stack.pop_back();
2104 Edge* e = v->edges;
2105 if (e)
2106 {
2107 do
2108 {
2109 if (e->target->copy != stamp)
2110 {
2111 e->target->copy = stamp;
2112 stack.push_back(e->target);
2113 }
2114 if (e->copy != stamp)
2115 {
2116 Face* face = facePool.newObject();
2117 face->init(e->target, e->reverse->prev->target, v);
2118 faces.push_back(face);
2119 Edge* f = e;
2120
2121 Vertex* a = NULL;
2122 Vertex* b = NULL;
2123 do
2124 {
2125 if (a && b)
2126 {
2127 int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
2128 btAssert(vol >= 0);
2129 Point32 c = v->point + a->point + b->point + ref;
2130 hullCenterX += vol * c.x;
2131 hullCenterY += vol * c.y;
2132 hullCenterZ += vol * c.z;
2133 volume += vol;
2134 }
2135
2136 btAssert(f->copy != stamp);
2137 f->copy = stamp;
2138 f->face = face;
2139
2140 a = b;
2141 b = f->target;
2142
2143 f = f->reverse->prev;
2144 } while (f != e);
2145 }
2146 e = e->next;
2147 } while (e != v->edges);
2148 }
2149 }
2150
2151 if (volume.getSign() <= 0)
2152 {
2153 return 0;
2154 }
2155
2156 btVector3 hullCenter;
2157 hullCenter[medAxis] = hullCenterX.toScalar();
2158 hullCenter[maxAxis] = hullCenterY.toScalar();
2159 hullCenter[minAxis] = hullCenterZ.toScalar();
2160 hullCenter /= 4 * volume.toScalar();
2161 hullCenter *= scaling;
2162
2163 int faceCount = faces.size();
2164
2165 if (clampAmount > 0)
2166 {
2167 btScalar minDist = SIMD_INFINITY;
2168 for (int i = 0; i < faceCount; i++)
2169 {
2170 btVector3 normal = getBtNormal(faces[i]);
2171 btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
2172 if (dist < minDist)
2173 {
2174 minDist = dist;
2175 }
2176 }
2177
2178 if (minDist <= 0)
2179 {
2180 return 0;
2181 }
2182
2183 amount = btMin(amount, minDist * clampAmount);
2184 }
2185
2186 unsigned int seed = 243703;
2187 for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
2188 {
2189 btSwap(faces[i], faces[seed % faceCount]);
2190 }
2191
2192 for (int i = 0; i < faceCount; i++)
2193 {
2194 if (!shiftFace(faces[i], amount, stack))
2195 {
2196 return -amount;
2197 }
2198 }
2199
2200 return amount;
2201 }
2202
shiftFace(Face * face,btScalar amount,btAlignedObjectArray<Vertex * > stack)2203 bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack)
2204 {
2205 btVector3 origShift = getBtNormal(face) * -amount;
2206 if (scaling[0] > 0)
2207 {
2208 origShift[0] /= scaling[0];
2209 }
2210 if (scaling[1] > 0)
2211 {
2212 origShift[1] /= scaling[1];
2213 }
2214 if (scaling[2] > 0)
2215 {
2216 origShift[2] /= scaling[2];
2217 }
2218 Point32 shift((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]);
2219 if (shift.isZero())
2220 {
2221 return true;
2222 }
2223 Point64 normal = face->getNormal();
2224 #ifdef DEBUG_CONVEX_HULL
2225 printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
2226 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);
2227 #endif
2228 int64_t origDot = face->origin.dot(normal);
2229 Point32 shiftedOrigin = face->origin + shift;
2230 int64_t shiftedDot = shiftedOrigin.dot(normal);
2231 btAssert(shiftedDot <= origDot);
2232 if (shiftedDot >= origDot)
2233 {
2234 return false;
2235 }
2236
2237 Edge* intersection = NULL;
2238
2239 Edge* startEdge = face->nearbyVertex->edges;
2240 #ifdef DEBUG_CONVEX_HULL
2241 printf("Start edge is ");
2242 startEdge->print();
2243 printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot);
2244 #endif
2245 Rational128 optDot = face->nearbyVertex->dot(normal);
2246 int cmp = optDot.compare(shiftedDot);
2247 #ifdef SHOW_ITERATIONS
2248 int n = 0;
2249 #endif
2250 if (cmp >= 0)
2251 {
2252 Edge* e = startEdge;
2253 do
2254 {
2255 #ifdef SHOW_ITERATIONS
2256 n++;
2257 #endif
2258 Rational128 dot = e->target->dot(normal);
2259 btAssert(dot.compare(origDot) <= 0);
2260 #ifdef DEBUG_CONVEX_HULL
2261 printf("Moving downwards, edge is ");
2262 e->print();
2263 printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
2264 #endif
2265 if (dot.compare(optDot) < 0)
2266 {
2267 int c = dot.compare(shiftedDot);
2268 optDot = dot;
2269 e = e->reverse;
2270 startEdge = e;
2271 if (c < 0)
2272 {
2273 intersection = e;
2274 break;
2275 }
2276 cmp = c;
2277 }
2278 e = e->prev;
2279 } while (e != startEdge);
2280
2281 if (!intersection)
2282 {
2283 return false;
2284 }
2285 }
2286 else
2287 {
2288 Edge* e = startEdge;
2289 do
2290 {
2291 #ifdef SHOW_ITERATIONS
2292 n++;
2293 #endif
2294 Rational128 dot = e->target->dot(normal);
2295 btAssert(dot.compare(origDot) <= 0);
2296 #ifdef DEBUG_CONVEX_HULL
2297 printf("Moving upwards, edge is ");
2298 e->print();
2299 printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot);
2300 #endif
2301 if (dot.compare(optDot) > 0)
2302 {
2303 cmp = dot.compare(shiftedDot);
2304 if (cmp >= 0)
2305 {
2306 intersection = e;
2307 break;
2308 }
2309 optDot = dot;
2310 e = e->reverse;
2311 startEdge = e;
2312 }
2313 e = e->prev;
2314 } while (e != startEdge);
2315
2316 if (!intersection)
2317 {
2318 return true;
2319 }
2320 }
2321
2322 #ifdef SHOW_ITERATIONS
2323 printf("Needed %d iterations to find initial intersection\n", n);
2324 #endif
2325
2326 if (cmp == 0)
2327 {
2328 Edge* e = intersection->reverse->next;
2329 #ifdef SHOW_ITERATIONS
2330 n = 0;
2331 #endif
2332 while (e->target->dot(normal).compare(shiftedDot) <= 0)
2333 {
2334 #ifdef SHOW_ITERATIONS
2335 n++;
2336 #endif
2337 e = e->next;
2338 if (e == intersection->reverse)
2339 {
2340 return true;
2341 }
2342 #ifdef DEBUG_CONVEX_HULL
2343 printf("Checking for outwards edge, current edge is ");
2344 e->print();
2345 printf("\n");
2346 #endif
2347 }
2348 #ifdef SHOW_ITERATIONS
2349 printf("Needed %d iterations to check for complete containment\n", n);
2350 #endif
2351 }
2352
2353 Edge* firstIntersection = NULL;
2354 Edge* faceEdge = NULL;
2355 Edge* firstFaceEdge = NULL;
2356
2357 #ifdef SHOW_ITERATIONS
2358 int m = 0;
2359 #endif
2360 while (true)
2361 {
2362 #ifdef SHOW_ITERATIONS
2363 m++;
2364 #endif
2365 #ifdef DEBUG_CONVEX_HULL
2366 printf("Intersecting edge is ");
2367 intersection->print();
2368 printf("\n");
2369 #endif
2370 if (cmp == 0)
2371 {
2372 Edge* e = intersection->reverse->next;
2373 startEdge = e;
2374 #ifdef SHOW_ITERATIONS
2375 n = 0;
2376 #endif
2377 while (true)
2378 {
2379 #ifdef SHOW_ITERATIONS
2380 n++;
2381 #endif
2382 if (e->target->dot(normal).compare(shiftedDot) >= 0)
2383 {
2384 break;
2385 }
2386 intersection = e->reverse;
2387 e = e->next;
2388 if (e == startEdge)
2389 {
2390 return true;
2391 }
2392 }
2393 #ifdef SHOW_ITERATIONS
2394 printf("Needed %d iterations to advance intersection\n", n);
2395 #endif
2396 }
2397
2398 #ifdef DEBUG_CONVEX_HULL
2399 printf("Advanced intersecting edge to ");
2400 intersection->print();
2401 printf(", cmp = %d\n", cmp);
2402 #endif
2403
2404 if (!firstIntersection)
2405 {
2406 firstIntersection = intersection;
2407 }
2408 else if (intersection == firstIntersection)
2409 {
2410 break;
2411 }
2412
2413 int prevCmp = cmp;
2414 Edge* prevIntersection = intersection;
2415 Edge* prevFaceEdge = faceEdge;
2416
2417 Edge* e = intersection->reverse;
2418 #ifdef SHOW_ITERATIONS
2419 n = 0;
2420 #endif
2421 while (true)
2422 {
2423 #ifdef SHOW_ITERATIONS
2424 n++;
2425 #endif
2426 e = e->reverse->prev;
2427 btAssert(e != intersection->reverse);
2428 cmp = e->target->dot(normal).compare(shiftedDot);
2429 #ifdef DEBUG_CONVEX_HULL
2430 printf("Testing edge ");
2431 e->print();
2432 printf(" -> cmp = %d\n", cmp);
2433 #endif
2434 if (cmp >= 0)
2435 {
2436 intersection = e;
2437 break;
2438 }
2439 }
2440 #ifdef SHOW_ITERATIONS
2441 printf("Needed %d iterations to find other intersection of face\n", n);
2442 #endif
2443
2444 if (cmp > 0)
2445 {
2446 Vertex* removed = intersection->target;
2447 e = intersection->reverse;
2448 if (e->prev == e)
2449 {
2450 removed->edges = NULL;
2451 }
2452 else
2453 {
2454 removed->edges = e->prev;
2455 e->prev->link(e->next);
2456 e->link(e);
2457 }
2458 #ifdef DEBUG_CONVEX_HULL
2459 printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2460 #endif
2461
2462 Point64 n0 = intersection->face->getNormal();
2463 Point64 n1 = intersection->reverse->face->getNormal();
2464 int64_t m00 = face->dir0.dot(n0);
2465 int64_t m01 = face->dir1.dot(n0);
2466 int64_t m10 = face->dir0.dot(n1);
2467 int64_t m11 = face->dir1.dot(n1);
2468 int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
2469 int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
2470 Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
2471 btAssert(det.getSign() != 0);
2472 Vertex* v = vertexPool.newObject();
2473 v->point.index = -1;
2474 v->copy = -1;
2475 v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01)
2476 + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
2477 Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01)
2478 + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
2479 Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01)
2480 + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
2481 det);
2482 v->point.x = (int32_t) v->point128.xvalue();
2483 v->point.y = (int32_t) v->point128.yvalue();
2484 v->point.z = (int32_t) v->point128.zvalue();
2485 intersection->target = v;
2486 v->edges = e;
2487
2488 stack.push_back(v);
2489 stack.push_back(removed);
2490 stack.push_back(NULL);
2491 }
2492
2493 if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target))
2494 {
2495 faceEdge = newEdgePair(prevIntersection->target, intersection->target);
2496 if (prevCmp == 0)
2497 {
2498 faceEdge->link(prevIntersection->reverse->next);
2499 }
2500 if ((prevCmp == 0) || prevFaceEdge)
2501 {
2502 prevIntersection->reverse->link(faceEdge);
2503 }
2504 if (cmp == 0)
2505 {
2506 intersection->reverse->prev->link(faceEdge->reverse);
2507 }
2508 faceEdge->reverse->link(intersection->reverse);
2509 }
2510 else
2511 {
2512 faceEdge = prevIntersection->reverse->next;
2513 }
2514
2515 if (prevFaceEdge)
2516 {
2517 if (prevCmp > 0)
2518 {
2519 faceEdge->link(prevFaceEdge->reverse);
2520 }
2521 else if (faceEdge != prevFaceEdge->reverse)
2522 {
2523 stack.push_back(prevFaceEdge->target);
2524 while (faceEdge->next != prevFaceEdge->reverse)
2525 {
2526 Vertex* removed = faceEdge->next->target;
2527 removeEdgePair(faceEdge->next);
2528 stack.push_back(removed);
2529 #ifdef DEBUG_CONVEX_HULL
2530 printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2531 #endif
2532 }
2533 stack.push_back(NULL);
2534 }
2535 }
2536 faceEdge->face = face;
2537 faceEdge->reverse->face = intersection->face;
2538
2539 if (!firstFaceEdge)
2540 {
2541 firstFaceEdge = faceEdge;
2542 }
2543 }
2544 #ifdef SHOW_ITERATIONS
2545 printf("Needed %d iterations to process all intersections\n", m);
2546 #endif
2547
2548 if (cmp > 0)
2549 {
2550 firstFaceEdge->reverse->target = faceEdge->target;
2551 firstIntersection->reverse->link(firstFaceEdge);
2552 firstFaceEdge->link(faceEdge->reverse);
2553 }
2554 else if (firstFaceEdge != faceEdge->reverse)
2555 {
2556 stack.push_back(faceEdge->target);
2557 while (firstFaceEdge->next != faceEdge->reverse)
2558 {
2559 Vertex* removed = firstFaceEdge->next->target;
2560 removeEdgePair(firstFaceEdge->next);
2561 stack.push_back(removed);
2562 #ifdef DEBUG_CONVEX_HULL
2563 printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
2564 #endif
2565 }
2566 stack.push_back(NULL);
2567 }
2568
2569 btAssert(stack.size() > 0);
2570 vertexList = stack[0];
2571
2572 #ifdef DEBUG_CONVEX_HULL
2573 printf("Removing part\n");
2574 #endif
2575 #ifdef SHOW_ITERATIONS
2576 n = 0;
2577 #endif
2578 int pos = 0;
2579 while (pos < stack.size())
2580 {
2581 int end = stack.size();
2582 while (pos < end)
2583 {
2584 Vertex* kept = stack[pos++];
2585 #ifdef DEBUG_CONVEX_HULL
2586 kept->print();
2587 #endif
2588 bool deeper = false;
2589 Vertex* removed;
2590 while ((removed = stack[pos++]) != NULL)
2591 {
2592 #ifdef SHOW_ITERATIONS
2593 n++;
2594 #endif
2595 kept->receiveNearbyFaces(removed);
2596 while (removed->edges)
2597 {
2598 if (!deeper)
2599 {
2600 deeper = true;
2601 stack.push_back(kept);
2602 }
2603 stack.push_back(removed->edges->target);
2604 removeEdgePair(removed->edges);
2605 }
2606 }
2607 if (deeper)
2608 {
2609 stack.push_back(NULL);
2610 }
2611 }
2612 }
2613 #ifdef SHOW_ITERATIONS
2614 printf("Needed %d iterations to remove part\n", n);
2615 #endif
2616
2617 stack.resize(0);
2618 face->origin = shiftedOrigin;
2619
2620 return true;
2621 }
2622
2623
getVertexCopy(btConvexHullInternal::Vertex * vertex,btAlignedObjectArray<btConvexHullInternal::Vertex * > & vertices)2624 static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices)
2625 {
2626 int index = vertex->copy;
2627 if (index < 0)
2628 {
2629 index = vertices.size();
2630 vertex->copy = index;
2631 vertices.push_back(vertex);
2632 #ifdef DEBUG_CONVEX_HULL
2633 printf("Vertex %d gets index *%d\n", vertex->point.index, index);
2634 #endif
2635 }
2636 return index;
2637 }
2638
compute(const void * coords,bool doubleCoords,int stride,int count,btScalar shrink,btScalar shrinkClamp)2639 btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
2640 {
2641 if (count <= 0)
2642 {
2643 vertices.clear();
2644 edges.clear();
2645 faces.clear();
2646 return 0;
2647 }
2648
2649 btConvexHullInternal hull;
2650 hull.compute(coords, doubleCoords, stride, count);
2651
2652 btScalar shift = 0;
2653 if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0))
2654 {
2655 vertices.clear();
2656 edges.clear();
2657 faces.clear();
2658 return shift;
2659 }
2660
2661 vertices.resize(0);
2662 edges.resize(0);
2663 faces.resize(0);
2664
2665 btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices;
2666 getVertexCopy(hull.vertexList, oldVertices);
2667 int copied = 0;
2668 while (copied < oldVertices.size())
2669 {
2670 btConvexHullInternal::Vertex* v = oldVertices[copied];
2671 vertices.push_back(hull.getCoordinates(v));
2672 btConvexHullInternal::Edge* firstEdge = v->edges;
2673 if (firstEdge)
2674 {
2675 int firstCopy = -1;
2676 int prevCopy = -1;
2677 btConvexHullInternal::Edge* e = firstEdge;
2678 do
2679 {
2680 if (e->copy < 0)
2681 {
2682 int s = edges.size();
2683 edges.push_back(Edge());
2684 edges.push_back(Edge());
2685 Edge* c = &edges[s];
2686 Edge* r = &edges[s + 1];
2687 e->copy = s;
2688 e->reverse->copy = s + 1;
2689 c->reverse = 1;
2690 r->reverse = -1;
2691 c->targetVertex = getVertexCopy(e->target, oldVertices);
2692 r->targetVertex = copied;
2693 #ifdef DEBUG_CONVEX_HULL
2694 printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex());
2695 #endif
2696 }
2697 if (prevCopy >= 0)
2698 {
2699 edges[e->copy].next = prevCopy - e->copy;
2700 }
2701 else
2702 {
2703 firstCopy = e->copy;
2704 }
2705 prevCopy = e->copy;
2706 e = e->next;
2707 } while (e != firstEdge);
2708 edges[firstCopy].next = prevCopy - firstCopy;
2709 }
2710 copied++;
2711 }
2712
2713 for (int i = 0; i < copied; i++)
2714 {
2715 btConvexHullInternal::Vertex* v = oldVertices[i];
2716 btConvexHullInternal::Edge* firstEdge = v->edges;
2717 if (firstEdge)
2718 {
2719 btConvexHullInternal::Edge* e = firstEdge;
2720 do
2721 {
2722 if (e->copy >= 0)
2723 {
2724 #ifdef DEBUG_CONVEX_HULL
2725 printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
2726 #endif
2727 faces.push_back(e->copy);
2728 btConvexHullInternal::Edge* f = e;
2729 do
2730 {
2731 #ifdef DEBUG_CONVEX_HULL
2732 printf(" Face *%d\n", edges[f->copy].getTargetVertex());
2733 #endif
2734 f->copy = -1;
2735 f = f->reverse->prev;
2736 } while (f != e);
2737 }
2738 e = e->next;
2739 } while (e != firstEdge);
2740 }
2741 }
2742
2743 return shift;
2744 }
2745
2746
2747
2748
2749
2750