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