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