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